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ABSTRACT 


The  Naval  Postgraduate  School  in  Monterey,  California  is 
currently  working  on  an  ongoing  project  for  research  in 
autonomous  underwater  vehicle  (AUV)  technology.  This  pro¬ 
ject  comprises  two  areas  of  research.  The  first  area  is 
research  conducted  on  the  system,  NPS  II  AUV.  The  second 
area  is  a  computer  simulation  of  the  actual  system.  One 
topic  which  is  vital  to  both  areas  is  three-dimensional  path 
planning.  The  concept  of  three-dimensional  path  planning  is 
on  the  order  of  magnitude  of  polynomial  time  and  current 
research  in  this  area  is  limited.  This  paper  reviews  my 
findings  and  submits  an  algorithm  which  finds  a  best  path  in 
a  three-dimensional  environment,  while  avoiding  all  known 
polyhedral  obstacles.  The  algorithm's  concept  is  to  reduce 
the  three-dimensional  world  to  a  series  of  two-dimensional 
representations,  allowing  the  algorithm  to  use  tangential 
lines  created  from  the  start  to  nodes  on  the  polygons  lying 
between  the  start  and  goal,  from  nodes  on  polygons  to  other 
polygon  nodes  and  finally,  from  polygon  nodes  to  the  goal. 
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I. 


INTRODUCTION 


A .  BACKGROUND 

The  last  few  years,  technology  has  advanced 
significantly  to  see  the  use  of  unmanned  vehicles 
substantially  increase  for  land,  sea  and  air  applications. 
With  regards  to  military  applications,  tacticians  have  used 
this  technology  to  improve  and  extend  the  range  and 
effectiveness  of  ^,ensors  and  weapons.  In  the  recent  Unj.ted 
States,  and  allied  coalition  forces'  invasion  of  Iraq  in 
DESERT  STORM,  both  land  and  sea  forces  used  remotely 
operated  planes  to  perform  reconnaissance  of  enemy  forces 
with  remarkable  success.  There  are  other  examples  of  the 
successful  use  of  remotely  operated  vehicles.  For  example, 
the  Israeli  Air  Force  successfully  used  remotely  operated 
vehicles  to  perform  reconnaissance  and  intelligence 
collection  of  enemy  strong  points  prior  to  air  campaigns 
conducted  in  1982,  resulting  in  zero  losses  for  the  Israelis 
and  substantial  damage  inflicted  on  enemy  forces.  (Floyd 
91)  . 

Besides  air  operations,  the  concept  of  remotely  operated 
vehicles  can  be  incorporated  in  naval  operations .  The 
United  States  Navy's  current  maritime  strategy  calls  for  its 
forces  to  be  forward  deployed  as  possible.  This  results  in 
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US  Naval  forces  operating  in  or  near  enemy  controlled 
waters.  However,  since  Soviet  strategies  for  both  land  and 
sea  operations  call  for  military  operations  to  be  planned 
for  and  conducted  in  an  in-depth  multi-layered  unit 
configuration  for  both  offensive  and  defensive  operations, 
the  use  of  a  remotely  operated  tethered  vehicle  with  a 
nearby  mother  ship  acting  as  the  brain  is  not  feasible. 
Therefore,  with  the  US  Navy's  current  strategy  coupled  with 
the  Soviet  Union's  strategy  of  multi-layered  unit 
configurations,  the  use  of  autonomous  underwater  vehicles 
(AUV)  for  potential  military  operations  could  significantly 
improve  the  US  Navy's  operational  capabilities.  These  AUVs 
must  possess  sufficient  onboard  sensors  as  well  as  control 
units  and  be  able  to  perform  a  preplanned  mission  without 
external  control.  (Robinson  86) 

Even  with  the  current  decline  of  the  Soviet  Union  as  a 
world  leader  due  to  its  internal  conflicts  and  the  decline 
of  its  satellite  countries,  the  use  of  AUV  technology  is 
still  a  viable  option.  This  became  apparent  with  the  events 
and  operations  of  DESERT  SHIELD  and  DESERT  STORM  in  the 
Persian  Gulf,  especially  those  dealing  with  mining  of  the 
gulf  by  the  Iraqi  naval  forces.  In  addition  to  using  an  AUV 
in  mine  warfare,  an  AUV  can  a’  ■»  be  used  in  many  different 
military  operations.  Examples  of  some  basic  types  of 
operations  follow: 
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1.  reconnaissance  (both  covert  and  overt)  for 
intelligence  gathering 

2.  surveillance 

3.  mine  warfare 

4 .  terrain  mapping 

5 .  supply  and  resupply  for  covert  units  along  the  coast 
munitions  delivery  (or  an  intelligent  torpedo) 

The  use  of  an  AUV  for  these  type  of  operations  would 
significantly  reduce  equipment  and  personnel  to  enemy 
capabilities,  thus  reducing  operational  costs  no  matter  the 
cost  of  the  AUV.  Advances  in  computer  technology, 
particularly  in  Artificial  Intelligence,  have  made  it 
possible  to  begin  AUV  development  for  these  type  of 
missions . 

However,  concept  of  traveling  from  A  to  B  without  human 
interaction  requires  that  the  system  act  and  think  as  a 
human  would.  One  particular  area  which  offers  significant 
challenges  is  path  planning  such  that  the  system  avoids  all 
known  obstacles  within  the  environment.  In  a  three- 
dimensional  environment,  the  challenge  is  even  more 
interesting  since  there  exists  an  infinite  amount  of 
possible  paths  extending  from  point  A  to  point  B.  This 
concept  of  three-dimensional  path  planning  is  also  not 
limited  to  an  AUV  system,  but  can  be  used  on  any  system 
which  must  traverse  a  three-dimensional  environment. 
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B .  AUV  CONCEPT 


The  basic  design  of  an  AUV  calls  for  an  unmanned 
submersible  vehicle  with  onboard  systems  and  sub-systems 
that  provide  power,  motion  control,  navigation,  obstacle 
detection  and  collision  avoidance  with  the  capability  to 
perform  a  preplanned  mission  by  controlling  and  monitoring 
onboard  systems  without  any  external  input.  In  addition, 
the  AUV  must  be  able  to  replan  its  mission  in  the  event  it 
encounters  unplanned  for  events.  This  includes  onboard  path 
replanning  to  the  mission  goal  in  the  event  the  AUV 
encounters  an  unplanned- for  obstacle  (Floyd  91) .  The  Naval 
Postgraduate  School  (NFS)  AUV  II  models  this  basic  concept 
of  AUV  development;  however,  an  AUV  capable  of  performing 
the  type  of  missions  listed  above  will  need  to  carry 
additional  equipment  tailored  to  perform  the  specific 
mission  in  order  to  accomplish  the  mission. 

C.  PATH  PLANNING  AND  REPLANNING 

The  concept  behind  path  planning  is  to  find  the  shortest 
path  or  a  relatively  short  path  from  an  initial  starting 
point  to  a  goal,  avoiding  all  known  obstacles  in  the 
environment.  This  concept  results  in  a  preplanned  mission 
which  is  then  stored  onboard  on  of  the  memory  modules  of  the 
AUV.  This  path  planning  process  can  be  accomplished  using 
the  AUV  control  unit,  but  since  this  process  is  a-priori  to 
the  mission  execution,  it  would  be  more  feasible  to  do  the 
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process  on  another  system  with  more  capability,  This  allows 
for  faster  results  determining  a  path,  multiple  path 
planning  missions  and  leaves  the  AUV  for  other  mission 
executions.  However,  the  replanning  concept  calls  for  the 
path  planning  process  to  be  performed  on  the  AUV.  The 
concept  of  an  onboard  mission  planner  further  implies  that 
the  planner  must  have  the  capability  to  access  onboard 
stored  information  of  the  environmental  model.  It  further 
implies  that  the  replanning  process  must  be  in  strict 
coordination  with  not  only  the  obstacle  avoidance  capability 
on  the  AUV,  but  also  the  module  for  location  determination. 
This  thesis  will  address  the  path  planning  and  replanning 
process.  Work  on  the  obstacle  avoidance  and  associated 
problems  has  been  addressed  by  another  NFS  AUV  II  project 
team  member  (Floyd  91). 

D.  OBJECTIVES 

This  thesis  will  address  the  following  research 
questions : 

1.  How  to  define  the  typical  underwater  environment? 

2.  Given  a  known  underwater  world  environment,  how  to  plan 
a  route  which  is  the  shortest  path  or  a  relatively 
short  path  and  avoids  all  known  polyhedrons  in  the 
environment? 

3.  How  to  incorporate  the  path  planner  as  a  on-board 
replanner? 
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E.  THESIS  ORGANIZATION 

The  organization  of  this  thesis  is  in  five  chapters. 
Chapter  II  contains  background  information  concerning  the 
NFS  AUV  II  system  along  with  a  general  summary  of  other  AUV 
projects  ongoing  as  well  as  research  conducted  on  path 
planning.  Chapters  III  and  IV  discuss  implementation  of  the 
path  planning  process . 

The  main  idea  behind  any  path  planning  algorithm  is  two 
fold.  First,  the  planner  must  determine  if  there  exists  any 
obstacles  lying  between  the  starting  point  and  the  ending  or 
goal  point.  If  there  are  not  any  obstacles,  then  the  path 
from  start  to  goal  is  the  straight  line  path  from  start  to 
goal.  However,  if  there  is  one  or  more  obstacles  between 
the  start  and  goal,  then  the  path  planner  must  begin  the 
planning  process  to  determine  the  shortest  of  a  relatively 
short  path. 

The  organization  of  this  thesis  follows  this  idea. 
Chapter  III  begins  with  a  discussion  of  how  the  three- 
dimensional  model  is  depicted.  Afterwards,  the  rest  of  the 
chapter  is  devoted  to  describing  the  technique  used  to 
determine  if  one  or  more  obstacles  lie  between  the  start  and 
goal.  Chapter  IV  describes  in  detail  the  path  planning 
process  used  to  determine  a  best  path  from  start  to  goal  if 
there  is  one  or  more  obstacle  between  them.  Finally, 

Chapter  V  are  my  conclusions  and  recommendations  for  future 
research. 
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II.  NPS  AUV  PROJECT  AND  RELATED  WORK 


A.  SURVEY  OF  PREVIOUS  WORKS 

As  stated  before,  the  primary  goal  of  any  AUV  research 
is  to  allow  the  vehicle  to  perform  a  mission  without  human 
intervention  during  the  execution  of  the  mission.  To 
operate  autonomously,  the  vehicle  must  possess  adequate 
intelligence  to  travel  a  pre-planned  route,  but  also  react 
to  unplanned  for  situations.  There  are  several  ongoing  AUV 
research  projects  as  well  as  research  for  path  planning  for 
both  two-dimensional  and  three  dimensional. 

1.  Autonomous  Underwater  Vehicle  Projects 

Within  the  past  decade,  research  on  Autonomous 
Vehicle  control  has  made  great  strides.  There  are  several 
ongoing  projects  for  AUV  development  conducted  by  not  only 
academic  institutions,  but  also  government  agencies  as  well. 

Texas  A&M  University  is  one  of  the  academic 
institutions  working  on  AUV  research.  Most  project  designs 
not  only  incorporate  the  software  or  an  on-board  computer 
controller,  but  also  the  specific  hardware  that  the 
controller  controls.  Texas  A&M  University  has  taken  a 
different  approach  in  the  project  development.  Their 
approach  is  to  develop  a  generic  computer  controller  capable 
of  successfully  performing  a  fully  autonomous  long  range 
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underwater  mission.  Table  I  shows  the  mission  requirements 
While  not  specifically  building  the  hardware  for  the 
controller  to  control,  a  generic  vehicle  was  considered  in 
order  to  provide  design  parameters  in  developing  the 
Table  I  -  TEXAS  A&M  MISSION  REQUIREMENTS 

MISSION  REQUIREMENTS 

Long  Range  Capability  -  3000  Nautical  Miles 
Long  Duration  -  300  Hours 
Precise  Navigation  Capability 
Collision  Avoidance 

Communication  Capability  with  Mother  Ship 
High  Reliability  and  Adaptability 

controller.  As  can  be  seen  from  Table  II,  the  vehicle 
requirements  are  unrealistic  for  any  type  of  testing  other 

Table  II  -  TEXAS  A&M  VEHICLE  CHARACTERISTICS 


VEHICLE 

CHARACTERISTICS 

Length 

81  feet 

Diameter 

13  feet 

Top  Speed 

12  knots 

Cruise 

10  knots 

Maximum  Depth 

800  feet 

Range 

3000  Nautical  Miles 

300  hours  (3  10  NM/Hour 

Fuel  Load 

8.1  tons  Diesel 

Computational  Power 

16  SUN  Sparc  Stations 

than  simulation  testing.  Texas  A&M  researchers  use  a  large 
vehicle  in  the  development  phase  with  the  idea  that  the 
generic  controller  can  be  adapted  for  other  vehicle  designs; 
however,  with  the  use  of  such  a  large  computer  platform  in 
the  design,  one  major  question  is  whether  the  controller  can 
be  adapted  to  a  platform  which  does  not  have  16  SUN  Sparc 
Stations  for  computational  power.  (TEXAS  A&M  90) 

The  United  States  Navy  in  conjunction  with  DARPA 
initiated  an  AUV  program  in  1988  with  the  primary  purpose  of 
showing  that  AUV  systems  could  successfully  perform  certain 
Navy  mission  requirements.  Table  III  shows  these  mission 
requirements  along  with  some  of  the  vehicle  characteristics . 
Unlike  the  Texas  A&M  project,  DARPA  is  developing  a  complete 
system  of  both  hardware  and  software  along  with  a  real-time 
simulation  capability  of  the  AUV.  And  like  the  Texas  A&M 
project,  DARPA 's  AUV  is  a  long  range  platform  and  not 
suitable  for  operations  within  a  small  area  such  as  a  harbor 
facility.  (Pappas  et.al.  91) 

Table  III  -  DARPA  MISSION  AND  VEHICLE  CHARACTERISTICS 

MISSION  AND  VEHICLE  CHARACTERISTICS 

1.  Mission 

a.  Tactical  Acoustic  System 

b.  Mine  Search  System 

c.  Remote  Surveillance  System 

2.  Vehicle  Characteristics 


a . 

Weight 

7.5  tons 

b. 

Depth 

1500  feet 

c . 

Speed 

10  knots 

d. 

Range 

240  Nautical  Miles 
24  hours  (3  10  knots 
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The  University  of  New  Hampshire  has  done  extensive 
research  on  AUV  technology.  There  initial  project  began  in 
1978  with  the  completion  of  the  first  Experimental 
Autonomous  Vehicle  (EAVE)  and  subsequent  successful  test  of 
autonomously  following  an  underwater  pipeline.  In  1983  this 
vehicle  successfully  followed  a  pre-defined  path  using 
acoustic  transponder  navigation.  In  1986,  the  university 
built  two  new  vehicles  similar  to  the  original,  but  with 
more  computational  power .  Current  interest  in  their 
research  is  to  use  artificial  intelligence  techniques  to 
develop  a  knowledge  based  guidance  and  control  system, 
(Blidberg  90) 

The  Institute  of  Industrial  Science  at  the 
University  of  Tokyo  has  developed  their  own  version  of  an 
AUV.  The  PTEROA150  was  built  and  successfully  tested  in 
1989.  After  successfully  testing  this  prototype,  the 
university  built  the  PTEROA250  with  greater  capabilities  as 
well  as  using  neural -net  technology  for  control  of  the 
vehicle.  The  Institutes  research  has  shown  that  a  neural 
net  controller  successfully  controls  the  AUV  and  can  be 
adjusted  to  allow  the  vehicle  to  be  used  in  different 
environmental  conditions.  (Ura  90) 

2 .  Path  Planning 

Articles  which  present  the  more  important  aspects  of 
path  planning  are  presented.  To  date,  there  has  been  a 
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considerable  amount  of  research  on  path  planning  in  a  two- 
dimensional  model.  However,  there  is  limited  amounted  of 
research  for  a  three-dimensional  model.  These  articles  can 
be  classified  into  two  main  categories  of  path  planning. 
These  categories  are  graph  searching  techniques  and 
potential  field  methods.  In  addition,  the  majority  of 
articles  that  do  deal  with  a  three-dimensional  model  find 
only  paths  based  on  the  shortest  path  and  do  not  consider  or 
simplify  other  constraints  such  as  time  allotted  for  travel, 
energy  consumption,  type  of  mission,  and  type  of 
environment . 

M.  Sharir  has  done  extended  research  in  the  area  of 
path  planning  and  obstacle  avoidance.  Sharir  presents 
several  algorithms  for  both  two-dimensional  and  three- 
dimensional  environments  (Sharir  87).  He  and  J.  T.  Schwartz 
have  also  collected  a  group  of  algorithms  on  motion  planning 
(Schwartz  88).  The  algorithms  they  present  are 
predominantly  graph  searching  techniques  and  their  work 
catalogs  the  algorithms  into  different  classes  based  on  each 
of  the  algorithms  properties.  Their  work  gives  a  good  basis 
for  further  research;  however,  as  they  point  out,  they 
present  no  research  on  algorithms  based  on  a  best  cost 
property  other  than  shortest  euclidean  distance  since  there 
is  very  little  research  ongoing. 

In  his  thesis,  Ong  describes  a  heuristic  search 
algorithm  that  finds  a  viable  path  in  a  three-dimensional 
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grid  coordinate  system.  Ong  uses  a  graph  searching  technique 
and  the  algorithm,  as  the  name  implies,  uses  detailed 
heuristic  to  prune  the  solution  space  by  selecting  only 
viable  successor  states  for  further  search.  The  heuristic 
that  are  used  allow  the  vehicle  to  model  the  human  decision¬ 
making  process  in  determining  which  viable  path  is  followed. 
Since  it  uses  heuristic  extensively,  it  can  be  considered  as 
an  informed  search  and  gives  a  semi-optimized  solution.  It 
uses  a  simplified  energy  consumption  rate  of  1.2  times  the 
overall  length  of  the  path  for  a  path  with  the  start  and 
goal  lying  at  different  depths.  For  a  path  with  turns,  the 
algorithm  adds  a  constant  for  the  size  of  the  turning  angle. 
As  the  size  of  the  turning  angle  increases,  the  larger  this 
cost  constant  becomes .  This  is  a  better  estimate  than  the 
estimate  than  the  estimate  used  for  depth  change;  however, 
it  is  not  accurate.  (Ong  89) 

Charles  Warren  uses  the  potential  fields  method  to 
determine  a  viable  path.  In  his  research,  Warren  uses 
artificial  potential  fields  in  a  global  path  planning  vice  a 
locally  selected  region.  The  idea  behind  his  research  is  to 
apply  potential  fields  around  configuration  space  (C-space) 
obstacles  and  use  these  fields  to  select  a  safe  path.  The 
first  step  in  the  process  is  to  map  the  environment  into  the 
C-space  of  the  robot.  Then  artificial  fields  are  placed 
around  the  obstacles  in  the  environment.  In  a  sea 
environment,  many  obstacles  begin  at  the  ground  and  extend 
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up.  Since  the  sea  floor  is  an  obstacle  as  well  as  these 
protrusions,  the  result  is  one  massive  obstacle.  Warren 
compensates  by  creating  the  potential  fields  from  horizontal 
planes  sliced  through  the  obstacles.  Each  of  these  planes 
forms  a  forbidden  region  similar  to  the  two-dimensional 
problem.  Since  Warren's  method  uses  a  global  method,  a 
chosen  path  is  selected  and  then  modified  using  the 
potential  fields  to  influence  its  direction.  An  advantage 
to  using  this  technique  is  that  the  probability  of  becoming 
trapped  by  a  local  minimum  is  greatly  reduced  since  the  path 
planning  process  uses  a  known  viable  path.  Warren's 
research  shows  a  viable  and  fast  path  planning  process . 
However,  it  has  its  limitations.  A  major  limitation  is  that 
the  algorithm  must  know  the  global  environment  prior  to  the 
process  beginning.  (Warren  89  and  Warren  90) 

Martin  Herman  introduces  a  fast  three-dimensional, 
collision -free  motion  planner  in  his  research  at  the 
National  Bureau  of  Standards.  In  his  planner,  Herman 
represents  the  environment  in  an  Octree  structure.  Using  an 
Octree  structure,  the  environment  is  initially  represented 
as  a  single  primitive  geometric  structure,  usually  a  cube. 
This  cube  is  the  root  of  a  octree.  Children  of  a  node  are 
created  if  the  parent  cubic  volume  is  not  homogeneous  or 
completely  filled  with  an  object  or  completely  empty.  If 
the  cubic  volume  is  not  homogeneous,  then  the  cube  is 
subdivided  into  eight  cubes  (called  octants)  with  each  cube 
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becoming  a  child  of  the  parent.  Once  the  environment  is 
represented  in  an  Octree  structure,  Herman  uses  one  of  three 
search  algorithms.  They  are  A*,  hill  climbing,  and 
hypothesize  and  test.  The  path  found  using  one  of  these 
algorithms  finds  a  collision-free  path,  but  it  is  not 
necessarily  the  shortest  path.  As  Herman  states,  finding 
the  shortest  path  is  computational  expensive  and  a 
relatively  short  path  is  adequate  for  many  tasks .  One  other 
major  limitation  to  this  method  of  path  planning  is  that 
considerable  memory  is  required  to  store  the  octree. 

(Herman  89) 

Perez  and  Wesley  have  introduced  in  their  research, 
an  algorithm  that  works  for  both  two-dimensional  and  three- 
dimensional  models.  In  determining  a  safe  shortest  path  for 
an  autonomous  vehicle,  the  authors'  algorithm  grows  each 
obstacle  by  some  parameter  that  corresponds  to  the 
dimensions  of  the  vehicle.  With  the  extra  dimensions  added 
to  each  obstacle,  the  best  path  is  assured  to  be  a  safe 
path.  The  algorithm  uses  a  parametric  function  to  allow  for 
vehicles  that  are  not  able  to  change  their  posture  in  the 
environment.  The  algorithm  uses  the  vertices,  represented 
in  the  x,  y  coordinate  system,  of  the  obstacle  in  the  two- 
dimensional  environment  in  finding  a  shortest  path.  In  the 
three-dimensional  environment,  the  grown  obstacles  are 
represented  in  the  x,  y,  z  coordinate  system.  However,  a 
shortest  path  whose  node  set  contains  only  vertices  of  these 
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giown  obstacles  will  not  necessarily  be  the  shortest  path. 

As  a  result,  the  authors  use  additional  vertices  of  a 
constant  distance  from  each  other.  The  resulting  path  is  a 
good  approximation  of  the  optimal  path.  (Perez  et.al.  79) 

Kanayama  has  introduced  work  on  two-dimensional  path 
planning  for  his  ongoing  research  with  the  Yamabico  Mobile 
Robot.  In  his  algorithm,  Kanayama  proposes  that  fast 
effective  path  planning  can  be  accomplished  using  tangent 
lines.  In  using  technique,  possible  paths  are  determined  by 
determining  the  tangent  lines  between  the  start  point  and 
all  possible  obstacles,  between  the  possible  obstacles  and 
other  obstacles  and  finally  the  obstacles  and  the  goal  point 
(Kanayama  90). 

There  has  been  a  significant  amount  of  research 
conducted  in  pruning  or  reducing  the  search  space  of  the 
visibility  graph.  Montgomery  has  introduced  several 
techniques  that  does  such.  One  of  more  interest  is  the 
technique  of  finding  the  farthest  points  to  the  left  and 
right  with  relation  to  the  point  being  considered  for 
expansion.  Once  these  points  are  found,  Montgomery's 
algorithm  determines  the  front  edge  of  the  obstacle  and 
strips  off  all  vertices  that  comprise  the  back  edge.  He 
argues  that  by  stripping  off  the  back  edges,  the 
computational  requirements  are  greatly  reduced  since  the 
number  of  vertices  is  reduced  by  roughly  a  factor  of  two. 

The  algorithm  also  strips  from  the  search  space  any 
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obstacles  that  are  occluded  from  the  expansion  point  by 
another  obstacle.  (Montgomery  et.al,  87) 

As  can  be  seen,  there  is  a  considerable  amount  of 
research  on  path  planning  in  a  three-dimensional 
environment.  Each  of  the  above  methods  are  viable  and 
efficient  as  a  motion  planner.  However,  these  methods  only 
use  a  shortest  path  (or  a  relatively  shortest  path)  to 
determine  which  path  is  the  best  path  to  use.  I  found  no 
research  which  addressed  using  some  other  measure  to 
determine  a  path  other  than  shortest  path. 

B.  THE  NFS  AUV  PROJECT 

The  NPS  AUV  PROJECT  is  currently  comprised  of  two 
different  areas  or  research.  The  first  area  research  is 
computer  simulation  of  the  overall  project  in  the 
laboratories  at  NPS;  the  second  area  is  experimental 
research  conducted  on  the  NPS  AUV  II,  using  the  NPS  swimming 
pool  as  the  world  environment. 

1.  The  NPS  AUV  II  Vehicle 

The  current  NPS  AUV  vehicle  is  called  the  NPS  AUV 
II.  Its  design  is  based  on  development  of  the  earlier  and 
smaller  prototype,  the  NPS  AUV  I  (Healey  et.al.  89).  The 
basic  vehicle  design  of  the  NPS  AUV  II  has  been  detailed  by 
Good  (Good  89).  Figure  1  illustrates  the  basic  design  of 
the  vehicle. 
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Sonar  transducers  Tlinncl  thrusters  Drive  motors 


Figure  1  -  NFS  II  AUV 


The  NFS  AUV  II  is  a  submersible  vehicle  capable  of  a 
maximum  speed  of  two  knots.  It  has  an  overall  length  of  93 
inches.  The  main  body  of  the  vehicle  is  made  of  aluminum 
and  the  constructed  as  a  box.  It  has  a  beam  of  16  inches,  a 
height  of  10  inches  and  a  length  of  72  inches.  The  nose 
cone  is  constructed  of  fiberglass  and  is  21  inches  long, 
extending  the  length  of  the  vehicle  to  93  inches.  The 
vehicle  displaces  380  pounds  and  uses  fixed  ballast.  The 
vehicle  has  four  forward  control  surfaces,  four  aft  control 
surfaces,  four  tunnel  thrusters  and  two  aft  screws  each 
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powered  by  24  volt  DC  drive  motors,  providing  1/8  horsepow¬ 
er.  With  the  eight  control  surfaces,  the  vehicle  is  highly 
maneuverable  and  has  a  turning  radius  of  3  ship  lengths . 

The  vehicle's  power  sources  are  lead-acid  gel  batteries 
capable  of  providing  up  to  two  hours  of  propulsion  and 
computational  power. 

On  board  sensors  include  the  following  systems : 

1.  a  navigation  system  sensor  comprised  of  a  flux  gate 
compass  and  directional  gyroscope,  a  vertical  gyroscope 
and  a  three  axis  rate  gyroscope  system  with  transla¬ 
tional  accelerometers . 

2.  a  paddle-wheel  speed  sensor  installed  in  the  nose. 

3.  four  sonar  transducers  installed  in  the  nose. 

The  on-board  computational  power  is  supplied  by  a  GESPAC 
MPU  30HZ  processor  with  a  Motorola  68030  CPU.  In  addition, 
the  system  also  has  two  megabyte  of  RAM  and  a  68882  math 
coprocessor  running  at  25  MHZ.  The  operating  system  is  the 
OS-9  multi-tasking  operating  system. 

2 .  Computer  Simulation 

The  computer  simulation  portion  of  the  NPS  AUV 
PROJECT  is  currently  done  on  a  high  resolution  graphics 
works - station .  The  basic  simulation  model  of  the  NPS  AUV  II 
was  developed  by  Jurewicz  (Jurewicz  91).  This  model  uses 
up-to-date  performance  coefficients  characteristic  of  the 
NPS  AUV  II.  The  basic  concept  of  the  simulator  is  to  allow 
project  team  members  to  integrate  individual  modules  of  the 
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project  into  the  simulator  for  testing  prior  to 
implementation  on-board  the  NPS  AUV  II.  The  following 
modules  have  been  implemented  on  the  simulator: 


1.  A  simple  locomotion  generator  for  generating  a  path 
given  a  set  of  reference  points  along  a  path.  This 
generator  is  restricted  to  a  path  in  which  the 
reference  points  of  the  inputted  set  lying  on  the  same 
horizontal  plane. 

2.  A  three-dimensional  guidance  control  module  (Magrino 
91) 

3 .  An  obstacle  detection  and  avoidance  module  using  sonar 
technology.  This  module  has  also  been  successfully 
implemented  on  the  NPS  AUV  II.  (Floyd  91) 
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III.  OBSTACLE  REPRESENTATION 


One  of  the  first  problems  I  had  to  solve  was  determining 
how  to  represent  each  three-dimensional  obstacle  in  the 
environmental  model  .  There  were  two  areas  I  had  to 
consider  in  deciding  how  to  do  the  representation. 

The  first  area  was  that  I  wanted  my  path  planner  to  use 
a  tangential  method  similar  to  the  way  Kanayama  and  Crane 
approach  two-dimensional  path  planning  (Kanayama  90  and 
Crane  91).  The  other  area  of  concern  was  that  I  wanted  the 
path  planner  to  be  compatible  with  not  only  the  NPS  pool 
environment  where  actual  testing  of  the  NPS  II  AUV  is 
conducted  but  also  the  Monterey  Bay,  which  will  be  included 
in  the  simulator  at  a  later  date. 

In  addition  to  considering  these  two  concerns,  I  also 
made  some  assumptions  of  the  type  of  obstacles  and  the  type 
of  environment.  One  such  assumption  I  made  concerning  the 
obstacles  was  that  all  polyhedrons  were  considered  to  be 
convex  in  nature.  The  assumption  I  made  about  the 
environment  was  that  if  a  path  existed  between  the  start  and 
the  goal,  it  was  not  a  path  that  first  retreated  in  the 
opposite  direction.  A  path  which  first  retreats  in  the 
opposite  direction  is  any  path  that  begins  in  a  direction  in 
the  x-y  plane  which  is  plus  or  minus  180  degrees  different 
from  the  vector  from  start  to  goal  in  the  x-y  plane. 
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The  bay  data  was  collected  in  two  different  resolutions. 
The  first  resolution  covered  a  large  area  and  used  a 
distance  between  depth  points  of  200  meters;  the  second 
resolution,  while  much  smaller  gave  accuracy  with  a  distance 
of  30  meters  between  depth  points.  With  the  location  and 
depth  known  for  each  point,  an  XYZ  coordinate  system  could 
be  used  in  computations.  In  addition,  as  with  the  bay  data, 
an  XYZ  coordinate  system  could  be  used  for  the  NPS  Pool  and 
its  environment . 

With  these  two  concerns  in  mind,  I  decided  to  represent 
each  obstacle  in  a  triply  linked  list  with  each  obstacle 
composing  the  main  link,  each  obstacle-face  composing  the 
obstacle  link  and  each  vertex  of  the  face  composing  the 
obstacle-face  link.  In  addition,  the  obstacle  list 
comprises  first  the  side  faces,  followed  by  the  top  face  if 
needed,  and  then  the  bottom  face  if  needed.  Also,  each 
obstacle  face  list  is  a  doubly  linked  list  and  the  vertices 
of  the  face  are  placed  in  a  counter-clockwise  order.  Figure 
2  shows  the  basic  concept  in  this  link  list  structure. 

The  size  of  the  link  list  can  become  quite  large 
especially  for  a  real  world  environment  such  as  the  Monterey 
Bay.  However,  the  size  of  this  link  list  can  be  pruned  once 
the  user  inputs  the  start  point  and  the  goal  point.  For 
example,  once  the  user  inputs  these  points,  the  algorithm 
can  prune  from  the  list  all  obstacles  which  lie  behind  the 
start  goal . 
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IV .  PATH  PLANNING 


If  the  visibility  check  between  the  start  and  goal 
points  determines  that  there  are  obstacles  lying  between  the 
two  points,  the  path  planning  process  begins.  With  path 
planning  in  a  two-dimensional  environment,  the  process  is 
relatively  simple  since  all  paths  are  on  the  same  plane  or 
surface.  However,  three-dimensional  path  planning  does  not 
have  the  same  characteristics.  The  three-dimensional 
environment  has  an  infinite  amount  of  planes  and  possible 
paths  to  consider.  As  a  result,  I  had  to  develop  a  process 
which  reduced  the  search  space  and  the  number  of  possible 
paths  to  consider  to  a  reasonable  amount.  In  developing  the 
algorithm,  I  considered  only  obstacles  which  rise  from  the 
floor  of  the  environment  and  are  convex.  By  considering 
only  these  type  of  obstacles,  I  do  not  have  to  consider  a 
possible  path  which  goes  underneath  an  obstacle.  Even 
though,  as  Sharir  states,  the  problem  still  approaches 
polynomial  time  in  order  of  magnitude  (Sharir  87). 

A.  BASIC  CONCEPTS  OF  THREE  DIMENSIONAL  PATH  PLANNING 
In  developing  an  algorithm  to  find  a  best  path  in  a 
three  dimensional  environment,  I  decided  to  extend 
Kanayama's  two-dimensional  path  planning  algorithm. 
Kanayama's  technique  develops  tangents  from  the  expansion 
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point  to  each  valid  two-dimensional  polygon  in  determining  a 
best  path  (Kanayama  90).  In  addition  to  using  expanding  on 
his  technique,  I  will  also  expand  on  Kanayama 's  terminology. 

1.  Polyhedron  Intersection 

In  order  to  solve  the  problem  of  determining  whether 
or  not  a  polyhedron  lies  between  an  expansion  point  and  the 
goal,  I  used  vector  calculus  and  developed  a  simply  ray 
tracing  algorithm  to  determine  visibility.  A  point  is 
visible  with  another  point  if  and  only  if  no  polyhedrons 
intersect  the  vector  formed  by  the  two  points .  This 
process  of  determining  if  two  points  are  visible  or  not  is 
not  only  used  at  the  beginning  of  the  path  planning  process 
with  the  start  and  goal  points  used,  but  also  each  time  the 
path  planning  process  expands  a  path.  Therefore,  for 
explanatory  reasons,  I  use  the  term  "Expanding  Point"  and 
its  variations  in  the  explanation  to  denote  the  first  point 
used.  Similarly,  I  use  "End  Point"  to  denote  the  point  the 
process  is  expanding  to. 

a.  General  Description. 

The  process  initially  finds  the  ray  formed  by  the 
expansion  point  and  the  goal.  From  the  ray,  the  process 
determines  the  vector.  Once  this  vector  is  found,  the 
process  finds  the  equation  of  the  plane  formed  by  the  first 
face  of  the  first  polyhedron  in  the  list  of  polyhedrons, 
determines  if  there  exists  an  intersection  between  the 
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expansion-goal  ray  and  the  plane,  and  if  one  exists, 
determines  if  the  intersection  point  lies  within  the 
boundaries  of  the  polyhedron  face.  If  the  intersection 
point  lies  between  the  boundaries,  the  process  ends 
notifying  the  calling  process  that  a  polyhedron  lies  between 
the  two  points.  Otherwise,  the  process  continues  checking 
each  face  of  each  polyhedron,  notifying  the  calling  process 
that  the  two  points  are  visible  to  each  other.  Table  IV 
depicts  the  pseudo  language  I  developed  for  the  process. 


Table  IV  -  PSEUDO  LANGUAGE  FOR  INTERSECT  POLYHEDRON 


intersect_polyhedron (ptl,  pt2,  3D_world) 

{ 

for (each  polyhedron  in  3D_world){ 
for (each  face  on  polyhedron)! 
plane  =  f ind_plane_equation( 3  points  on  face); 
intersection_pt  =  f ind_intersection_point (ptl,  pt2, 

plane) ; 

if (intersection_pt  }=  NO_INTERSECTION) { 

if (lines_intersection( intersection,  face,  plane) 
return ( INTERSECT ) ; 

} 

) 

) 

return (NO_INTERSECT) : 


Jb.  Directed  Ray. 

The  process  begins  by  determining  the  parametric 
equations  for  the  ray  formed  using  the  expansion  point  and 
the  goal.  I  directed  the  ray  from  the  expansion  point  and 
the  goal  so  the  equations  take  on  the  form  in  Equation  1. 
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Eq  1 


X  =  +  (X,  -  x^)t 

y  =  y*  +  (y,  -  Ye)  t 

Z  =  +  {Zg  -  Z^)t 

However,  in  this  case,  the  direction  of  the  ray  does  not 
matter  and  adopted  this  manner  for  consistency  and 
readability. 

c.  Plane  Equation. 

In  determining  the  equation  of  a  plane  formed  by 
a  polyhedron  face,  the  process  uses  3  points  {(Xi,yi,Zi), 
(X2,y2,Z2),  (X3,y3,Z3))  from  the  polyhedron  face  the  process 
is  checking.  The  process  uses  these  3  points  to  determine  2 
vectors  in  the  form  of  Equations  2  and  3 . 

Vi  =  (Xj  -  Xi)  J  +  (yj  -  y^)J  +  (Zg  -  Zi)li:  Eq  2 

=  (Xj  -  Xj)  J  +  (yj  -  y-^)J  *  (Z3  -  Z^)K  Eq  3 

Once  these  two  vectors  are  found,  the  process  finds  the 
normal  vector  to  the  plane  by  determining  the  dot  product  of 
the  two  vectors  as  seen  in  Equation  4 . 

nr  =  Vi  •  ITj  Eq  4 

N  also  takes  on  the  form 

N  =  n^I  +  +  n^K  Eq  5 

Using  Equation  5,  the  equation  of  the  plane  can  be  found  by 

substituting  a  point  on  the  plane  into  the  equation.  If  the 

point  (X3,yi,Zi)  is  used,  the  result  is  Equation  6. 

J2i(x  -  Xj)  +  -  y-i)  +  /ijCz  -  Zi)  =0  Eq  6 

Equation  6  equates  to  Equation  7 

nix  +  n2y  +  n^z  -  D  -  0  Eq  7 
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where  D  equates  to  Equation  8 . 

D  -  +  n2y2  +  Eq  8 

The  source  code  I  developed  to  find  the  plane  equation  is 
shown  in  Table  V  and  in  Appendix  C. 


Table  V  -  FIND  PLANE  EQUATION  SOURCE  CODE 

equation  f ind_plane_equation (ptl ,  pt2 ,  pt3 ) 

point  ptl,  pt2,  pt3; 

{ 

point  vector 1_2 ,  vector 1_3 ; 

equation  pl_eq; 

vector l_2.x  =  pt2.x  -  ptl.x; 
vector l_2.y  =  pt2,y  -  ptl.y; 
vector l_2.z  =  pt2.z  -  ptl.z; 
vector l_3.x  =  pt3.x  -  ptl.x; 
vectorl_3.y  =  pt3.y  -  ptl.y; 
vector l_3.z  =  pt3.z  -  ptl.z; 

/♦cross  product  is  the  standard  cross  product 
equation.  Function  also  is  in  3dd.c*/ 
pl_eq  *  cross_product (vectorl_2,  vector 1_3 ) ; 
pl_eq.d  =  ((pl_eq.x  *  (-1.0)  *  ptl.x)  + 

(pl_eq.y  *  (-1.0)  *  ptl.y)); 
pl_eq.d  =  pl_eq.d  +  (pl_eq.z  *  (-1.0)  *  ptl.z); 
return  (pl_eq); 


d.  Intersection  Point. 

Since  the  process  needs  to  find  the  intersection 
point  formed  by  the  ray  and  the  plane,  the  values  for  x,  y, 
and  z  used  in  Equation  1  can  be  substituted  for  x,  y,  and  z 
in  Equation  7.  This  results  in  Equation  9. 
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+ 


Eq  9 


(x,  +  {Xg  -  x^)  t  -  Xj)  + 

^2  (y.  +  ^yg  -  y«)  t  -  yi) 

n,  +  (2^  -  2^)  t  -  2i)  =  0 


Finally,  Equation  10  results  when  t  is  solved. 


t  *  -■ 


jJiX,  +  *  iijy,  +  z? 


Eq  10 


73i  (x^  +  X.)  +  iVg  +  y.)  +  (2^  +  2^ 

If  v:e  let  w  and  Y  take  the  form  of  Equations  11 
and  12,  then  t  equates  to  Equation  13 

w  -  n^x^  +  fljX^  *  D  Eq  11 

Eq  12 
Eq  13 


y  =  Jii  Ug  -  X.)  +  Wg  -  y.)  *  n^^iZg  -  Zg) 

y 


t  =  -— 


In  equating  t  to  Equation  13,  there  is  a  check  in 
the  value  of  y.  By  first  computing  y  and  determining  if 
y  =  0,  then  the  ray  is  parallel  to  the  plane,  no  other 
computations  are  necessary,  and  the  function  returns  to  the 
calling  function  that  there  is  no  intersection  for  the 
checked  plane  face.  If  y  !=  0,  then  the  process  finds  the 
value  for  the  parameter  w. 

If  0  <  t  ^  1,  then  the  ray  intersects  the  plane 
between  the  two  points.  Ift<0ort>l,  then  the  ray 
intersects  the  plane,  but  not  on  the  line  segment  formed  by 
the  two  points.  If  the  process  determines  there  is  an 
intersection,  the  value  of  t  is  used  in  Equation  1  to 
calculate  the  intersection  point  and  the  process  returns  the 
point  to  the  calling  function.  Table  VI  shows  the  source 
code  I  developed  for  the  process .  The  source  code  is  also 
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in  Appendix  C,  which  contains  the  source  code  of  function ‘ 
which  deal  with  the  three-dimensional  environment. 


Table  VI  -  INTERSECTION  POINT  SOURCE  CODE 


point 

{ 


intersection_point (point  ptl,  point  pt2 , 

equation  plane) 


double 


y  =  0.0,  t  =  0.0,  w  =  0.0; 


y  =  (plane. X  *  (pt2.x  -  ptl.x)  +  plane. y  * 

(pt2.y  -  ptl.y)  +  (plane. z  *  (pt2.z  -  ptl.z)); 

if  ( !y)  {  /*  y  ==  0*/ 

intersection . X  =  NO_INTERSECTION; 

1 

else{ 

w  =  -(plane. X  *  ptl.x  +  plane. y  *  ptl.y  +  plane. z  * 
ptl.z  +  plane. d); 
t  =  w  /  y; 


if  (0.0  <=  t  &&  t 

<=  1.0) 

{ 

intersection .X 

=  ptl.x 

+ 

(pt2 .X 

-  ptl.x) 

*  t; 

intersection . y 

=  ptl.y 

4“ 

(pt2.y 

-  ptl.y) 

*  t; 

intersection . z 

=  ptl.z 

+ 

(pt2 . z 

-  ptl.z) 

*  t; 

) 


else 

intersection . X  =  NO_INTERSECTION; 

1 

return  ( intersection ) ; 


e.  Inside  Polyhedron  Check. 

If  the  process  determines  that  there  is  a  valid 
intersection  point,  it  then  determines  if  the  point  lies 
within  the  boundaries  of  the  polyhedron.  To  determine  this, 
the  process  projects  a  ray  from  the  point  to  a  point  outside 
the  search  space,  but  lying  on  the  same  x  y  z  plane  as  the 
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polyhedron  face.  The  process  then  determines  the  parametric 
equations  of  this  ray.  Next,  by  finding  the  parametric 
equations  for  each  boundary  segment  of  the  polyhedron  face, 
we  can  equate  the  two  sets  of  parametric  equations.  Since 
there  are  two  unknowns  (both  parameters),  the  process  uses 
two  of  the  three  equations  to  simultaneously  solve  for  the 
unknowns . 

For  example,  if  the  points  of  the  ray  are 
(Ui,  Vj,  Wi)  and  (Uj,  Vj,  Wj),  the  parametric  equations  are 
shown  in  Equation  14 . 

X  =  +  (Uj  -  Uj)  s 

y  =  +  ( Vg  -  v^)  s  Eq  14 

z  +  (Wj  - 


Similarly,  if  the  two  end  nodes  of  the  boundary  of  the 
polyhedron  face  are  (aj,  b^,  Cj)  and  (aj,  bj,  Cj) ,  the  results 
are  shown  in  Equation  15. 

X  =  +  (aj  -  a^) s 

y  =  +  (i?2  -  s  Eq  15 

Z  =  Cj  +  (C2  -  Cj)  s 

If  we  use  the  first  two  equations  of  Equations  14  and  15,  we 
can  solve  for  s  and  t.  Equations  16  and  17  depict  the 
results . 


t  =  ( (^1  -  Vi)  (^2  -  a,) )  -  ( (a^  -  u,)  (i?2  -  -bi) )  ^ 

((a^  -  a^)  (Vj  -  v^))  -  ((1>2  -  •bi)  (^2  ~ 

«  =  ((^1  -  vj)  (U2  -  Uj))  -  -  u^)  (V2  -  Vi))  £  17 

( (aj  -  a^)  (Vj  -  )  -  ( (2>2  -  ^i>  (^2  -  Ui> ) 

If  0  <  s  ^  1  and  0  <  t  <  1  are  valid  statements, 
then  the  ray  intersects  the  polyhedron  face.  This  process 
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must  be  done  for  each  boundary  of  the  face  with  a  counter 
counting  the  number  of  times  the  ray  intersects  the 
boundary,  If  the  counter  is  an  even  number,  then  the 
intersection  point  is  outside  of  the  boundaries  of  the  face 
and  thus  the  projection  from  the  expansion  point  to  the  goal 
does  not  intersect  the  polyhedron  face.  Conversely,  if  the 
count  is  odd,  the  start  goal  ray  intersects  the  polyhedron 
face . 


An  easy  implementation  of  determining  if  the 
counter  is  odd  or  even  is  to  use  modulo  arithmetic.  The 
pseudo  language  in  Table  VII  depicts  this  process.  The 
source  code  can  again  be  found  in  Appendix  C. 


Table  VII  -  PSEUDO  LANGUAGE  FOR  POLYHEDRON  INTERSECTION 


if  (line_intersection_count  modula  2  ==  1)  then 
polyhedron_intersection  =  True 

else 

polyhedron_intersection  =  False 

end  if 


As  mention  above,  the  process  checks  each  face  of 
each  polyhedron.  If  there  is  not  an  intersection  between 
the  vector  and  any  of  the  polyhedron  faces,  the  calling 
process  is  notified  by  passing  it  a  non -intersection  flag. 
However,  if  there  is  an  intersection,  the  process  stops 
checking  the  remaining  faces  and  notifies  the  calling 
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process  of  the  intersection  so  that  the  path  planning 
process  can  begin. 

2 .  Plane  Types 

The  use  of  the  plane  is  used  several  times 
throughout  the  process  of  finding  the  best  path.  There  are 
two  different  types  of  planes  used  in  finding  the  best  path. 
I  refer  to  one  of  the  two  types  of  planes  used  as  a 
"vertical  plane" .  The  other  plane  is  the  "horizontal 
plane".  I  refer  to  a  vertical  plane  as  such  because  two  of 
the  three  points  lie  on  the  same  X  Z  plane.  I  refer  to  a 
horizontal  plane  as  such  because  two  of  the  three  points  are 
on  the  same  X  Y  plane  and  it  is  perpendicular  to  the 
vertical  plane  formed  by  the  same  expansion  and  end  points. 
The  process  of  finding  the  equation  for  both  types  are  the 
same  and  uses  the  same  process  described  earlier  in  this 
chapter.  Appendix  C  contains  the  source  code  which  finds 
both  of  these  planes . 

3 .  Two-Dimensional  Polygons 

Two-dimensional  polygons  are  the  type  of  obstacles 
which  are  used  primarily  for  the  path  planning  process.  The 
general  concept  in  building  the  two-dimensional  world,  the 
Build_two_d_polygon_list  function,  cycles  through  each  face 
of  each  polyhedron,  finding  the  intersection  points  of  the 
horizontal  plane  and  the  line  segments  of  the  polyhedron 
face  if  one  exits.  If  an  intersection  does  exist,  then  the 
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process  puts  the  point  into  a  link  list  for  the  two- 
dimensional  list.  Figure  3  depicts  the  structure  of  this 
link  list  and  Table  VIII  shows  the  pseudo  language  used  to 
develop  the  list.  The  source  code  can  be  found  in  Appendix 
B. 


Figure  3  -  2D  Polygon  List 


The  Next  Polygon  entry  is  a  pointer  which  points  at 
the  next  polygon  in  the  linked  list.  The  Polyhedron  Number 
entry  is  the  number  of  the  polyhedron  which  the  algorithm 
used  to  find  the  polygon.  The  Polygon  Number  entry  is  the 
number  associated  to  when  the  polygon  was  formed.  The 
Polygon  Start  entry  is  a  pointer  which  points  to  the  first 
node  or  vertex  of  the  polygon .  The  nodes  are  in  a  doubly 
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linked  list  with  "ccw"  depicting  counter  clockwise  and  "cw" 
meaning  clockwise  in  order. 


Table  VIII  -  BUILD  TWO-DIMENSIONAL  POLYGON 


Build_two_d_polygon_list ( PolyhedronList ,  Plane ) 

I 

initialize  f irst_polygon; 
current_polygon  =  f  irst__polygon 
for (Each  Polyhedron  in  PolyhedronList ) { 
for (Each  Face  on  Polyhedron)! 

for (Each  Line  Segment  of  the  Face){ 

if (( intersection  =  intersection_point (nodel, 

node2,  plane)  !=  NO_INTERSECTION) 
Create_polygon ( intersection , 

current_polygon ) ; 

} 

) 

remove_duplicate_node ( current_polygon ) ; 
initialize  next_polygon; 
current_polygon  =  next_polygon ; 

} 

return ( polygon_lis t ) ; 


4 .  Number  and  Types  of  Paths 

In  the  two-dimensional  environment,  the  number  of 
possible  paths  is  2"  where  n  is  the  number  of  obstacles  in 
the  environment.  As  already  stated,  I  do  not  consider  paths 
which  go  underneath  obstacles.  As  a  result,  the  number  of 
possible  paths  in  the  three-dimensional  environment  is  3". 

In  using  Kanayama's  terminology  where  a  plus  sign  (+) 
denotes  a  path  extending  counter  clockwise  around  an 
obstacle  and  a  minus  sign  (-)  denotes  a  path  extending 
clockwise,  I  extend  it  to  include  an  up  arrow  (T)  which 
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denotes  a  path  extending  over  the  obstacle.  Table  VII  shows 
the  possible  paths  for  an  environment  with  1,  2  and  3 
obstacles . 

In  analyzing  these  possible  paths  in  Table  IX,  one 
can  see  that  general  paths  with  no  up  arrows  indicate  that 
the  path  lies  on  the  same  plane  as  the  start  and  goal .  In 
addition,  there  is  one  path  which  extends  above  all  the 
obstacles.  Thus,  all  other  paths  are  a  combination  of  the 
two.  In  further  analysis  of  Table  IX,  these  paths  can  be 
generalize  to  include  all  cases.  These  cases  are: 

1.  The  path  lies  on  the  same  plane  as  the  start  and  goal. 

2 .  The  path  goes  over  all  the  obstacles . 

3 .  The  path  goes  over  one  or  more  obstacles  before  going 
around  the  remaining  one  or  more  obstacles. 

4 .  The  path  goes  around  one  or  more  obstacles  before  going 
over  the  remaining  one  or  more  obstacles. 

5.  The  path  goes  over  one  or  more  obstacles,  goes  around 
one  or  more  obstacles  and  then  goes  over  the  remaining 
one  or  more  obstacles  or  begins  the  sequence  of  around 
and  back  over  the  remaining  obstacles  until  the  path 
reaches  the  goal . 

6 .  The  path  goes  around  one  or  mo  e  obstacles ,  goes  over 
one  or  more  obstacles  and  then  goes  around  the 
remaining  one  or  more  obstacles  or  begins  the  sequence 
of  over  and  back  around  the  remaining  obstacles  until 
the  path  reaches  the  goal. 

I  could  argue  that  bullets  3  and  4  are  subsets  of 
bullets  5  and  6  respectively.  However,  the  concept  of 
three-dimensional  path  planning  is  a  difficult  one. 
Therefore,  I  have  broken  the  types  of  paths  as  above. 
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allowing  me  to  explain  in  a  more  forthright  and  detailed 
manner  the  process  I  developed  to  find  the  best  path. 


Table  IX  -  POSSIBLE  PATHS  FOR  POLYHEDRON  WORLD 


1  OBSTACLE 

2  OBSTACLES 

3  OBSTACLES 

IM 

B) 

(A,  B,  C) 

At 

A+B+ 

A+B+C+ 

A- 

A+B- 

A+B+C- 

aT 

a+bT 

A+B+Ct 

A-B+ 

A+B-C+ 

A-B- 

A+B-C- 

a-bT 

a+b-cT 

aTb+ 

a+bTc+ 

aTb- 

a+bTc- 

TOTAL  POSSIBLE  PATHS 

aTbT 

a+bTcT 

A-B+C+ 

A-B+C- 

A-B+Ct 

A-B-C+ 

A-B-C- 

A-B-Ct 

a-bTc+ 

a-bTc- 

a-bTcT 

aTb+c+ 

aTb+c- 

aTb+cT 

aTb-c+ 

aTb-c- 

aTb-cT 

aTbTc+ 

aTbTc- 

aTbTcT 

3 

9 

27 
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5 .  Tangents 

As  with  Kanayama's  two-dimensional  path  planning 
algorithm,  my  process  uses  tangent  lines  extended  from  the 
start  point  or  an  expansion  point  to  all  valid  obstacles. 
However,  my  algorithm  extends  these  tangent  lines  to  not 
only  the  sides  of  the  obstacle,  but  also  to  the  top  of  the 
obstacle . 

These  two  basic  tangent  types  the  process  uses  in 
the  path  planning  process  can  be  classified  as  either 
horizontal  or  vertical  tangents.  I  further  sub-classify  the 
horizontal  tangent  as  either  a  plus  tangent  or  a  minus 
tangent . 

a.  Plus  and  Minus  Tangents 

I  define  a  tangent  to  a  three-dimensional 
polyhedron  to  be  the  ray  which  is  formed  by  the  expansion 
point  and  an  intersection  point  on  an  edge  of  the  polyhedron 
such  that  the  ray  intersects  the  polyhedron  only  at  the 
intersection  point.  For  the  case  where  a  tangent  crosses  a 
face  of  a  polyhedron,  then  the  first  intersection  point  is 
the  considered  the  tangent  point.  As  in  Kanayama's 
algorithm  for  the  two-dimensional  environment,  I  define  a 
plus  tangent  to  be  a  tangent  which  if  it  was  extended  around 
the  obstacle  so  that  it  encircled  the  obstacle,  it  would 
wrap  around  the  obstacle  in  a  counter-clockwise  direction. 
Conversely,  a  minus  tangent  is  a  tangent  which  if  the 
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tangent  encircled  the  obstacle,  it  would  wrap  around  the 


obstacle  in  a  clockwise  direction.  Figure  4  shows  the 
different  plus  tangents  for  a  two-dimensional  polygon. 
Similarly,  Figure  5  shows  the  different  minus  tangents 


Figure  4  -  Plus  Tangents  Figure  5  -  Minus  Tangents 


Up  until  now,  I  have  shown  the  plus  and  minus 


tangents  for  a  two-dimensional  polygon.  But  as  Figures  6 
and  7  show,  there  are  an  infinite  amount  of  plus  and  minus 


Figure  6  -  Plus  Tangents  for  Figure  7  -  Minus  Tangents  for 


Polyhedron  World  Polyhedron  World 

tangents  when  dealing  with  a  three-dimensional  environment. 
However,  this  unmanageable  amount  can  be  reduced  to  a  more 
manageable  amount  determined  by  the  type  of  path  being 


expanded  on  and  the  number  of  polyhedrons  within  the 
environment . 

The  general  idea  behind  this  reduction  is  to  find 
the  polygon  formed  by  the  intersection  of  a  plane  and  the 
polyhedron.  By  using  this  plane  to  find  all  the  polygons 
formed  by  the  intersection  of  the  plane  and  the  polyhedrons 
in  the  environment,  the  three-dimensional  world  is  reduced 
to  a  two-dimensional  representation.  Of  course,  the  number 
of  plane  slices  needed  will  be  different  for  each  of  the 
types  of  paths  as  listed  above.  See  Figure  8  for  a 
graphical  representation  of  this  intersection. 


I  use  Kanayama's  basic  algorithm  for  finding  each 
of  the  type  of  tangents  from  a  point  to  a  polygon.  However, 
I  expand  on  it  to  find  only  the  closest  tangent  point  to  the 
expansion  point.  This  case  must  be  accounted  for  when  the 
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expansion  point  and  one  of  the  sides  of  the  polygon  lie  on 
the  same  projected  ray.  Table  X  shows  the  pseudo  language 
Kanayama  has  developed  to  find  the  plus  tangent  (Kanayama 
90).  The  minus  tangent  code  is  the  same  except  that  the 
"if"  structure  which  checks  the  sign  and  order  is  evaluated 
with  -1  and  not  1.  The  sign  function  accepts  an  integer 
which  the  order  function  returns  and  returns  either  a  1  or  - 
1  based  on  whether  or  not  the  function  is  positive  or 
negative.  The  order  function  is  a  modification  of  the  area 
of  a  triangle  equation.  The  function  determines  the  order, 
either  clockwise  or  counter-clockwise,  of  the  three  points. 
For  further  discussion,  see  Kanayama 's  development  of  the 
function  (Kanayama  90). 


Table  X  -  PLUS  TANGENT  PSEUDO  LANGUAGE 


plus_tangent (pt,  polygon_A) 

{ 

q  =  initial_node(polygon_A) ; 
doforever { 

if (sign(order(pt,  q,  next(q) )  ==  1)  q  =  next(q); 
else{ 

if (sign(order(pt,  q,  prev(q))  ==  1)  q  =  prev{q); 
else  break; 

) 

) 

return ( q ) ; 


In  determining  if  the  expansion  point  lies  on  the 
same  project  ray  as  a  side  of  a  polygon,  the  process  must 
make  two  checks.  The  first  check  determines  if  the 
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direction  of  the  ray  formed  by  the  expansion  point  and  the 
point  found  by  the  plus  or  minus  tangent  function  is  equal 
to  the  ray  formed  by  the  expansion  point  and  the  vertex  node 
counter-clockwise  to  the  expansion  point.  Similarly,  the 
second  check  determines  if  the  ray  formed  by  the  expansion 
point  and  the  vertex  node  clockwise  to  the  point  is  equal  to 
the  ray  formed  by  the  tangent  point  and  expansion  node.  If 
the  rays  are  equal,  then  the  vertex  node  which  is  closest  to 
the  expansion  point  is  determined  and  returned.  Figure  9 
shows  these  scenarios.  Table  XI  shows  excerpts  of  the  C 
code  I  developed  to  accomplish  this.  Appendix  I  gives  the 
complete  source  code  for  both  the  plus  and  minus  tangent 
functions . 


MMUSTANGENT  PtUSTANGENT 


Figure  9  -  Direction  Heuristic  Check 


41 


Table  XI  -  DETERMINING  CLOSEST  TANGENT  POINT  SOURCE  CODE 


The  case  of  finding  the  tangent  from  one  polygon 
to  another  polygon  needs  to  be  handled  in  a  different 
manner.  The  problem  encountered  in  finding  this  type  of 
tangent  happens  when  the  tangent  node  from  the  first  polygon 
is  not  the  point  on  the  polygon  which  the  process  is 
expanding  from.  Figure  10  depicts  this  case. 

As  can  be  seen  in  Figure  10,  if  the  plus  tangent 
is  found  using  the  expansion  point,  the  tangent  line 
(Expansion  Point,  A)  cuts  across  the  first  polygon,  which 
cannot  occur  for  a  valid  partial  path.  The  same  thing 
occurs  when  trying  to  find  a  minus  tangent.  As  a  result,  I 
had  to  develop  a  process  which  found  the  appropriate 
tangents  between  the  polygons  (tangent  lines  (C,B)  and  (E,D) 
respectively) . 
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Polygons 

In  finding  the  two  appropriate  tangent  nodes  on 
the  polygons,  I  had  to  consider  what  type  of  tangent  was 
used  in  arriving  at  the  expansion  point.  The  idea  behind 
the  process  is  then  to  find  the  tangent  node  on  the  second 
polygon  from  the  expansion  point.  This  tangent  node  is  then 
used  in  finding  the  tangent  node  on  the  first  polygon.  This 
process  of  oscillating  from  one  polygon  to  the  next  polygon 
continues  as  long  as  the  tangent  line  intersects  one  of  the 
two  polygons.  Upon  finding  the  two  points  the  process 
returns  the  two  nodes  in  a  link  list  to  the  calling 
function.  Table  XII  depicts  this  process  in  pseudo  language 
for  each  of  the  different  cases  in  finding  the  appropriate 
tangent.  Appendix  I  contains  the  function  "tangent"  written 
in  the  C  language,  which  also  has  all  the  cases. 
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Table  XII  -  FINDING  APPROPRIATE  TANGENTS 
polygonl_pt  =  expansion_pt; 

/♦expansion  pt  tangent  is  minus 
plus  tangent  to  polygon2*/ 
do  { 

polygon2_pt  =  plus_tangent (polygon2 ,  polygonl_pt ) ; 
polygonl_pt  =  plus_tangent (polygonl,  polygon2_pt ) ; 

}while  (not (visible (polygonl_pt,  polygon2_pt ) ; 

/♦expansion  pt  tangent  is  minus 
minus  tangent  to  polygon  2*/ 
do  { 

polygon2_pt  =  minus_tangent (polygon2 ,  polygonl_pt ) ; 
polygonl_pt  =  plus_tangent (polygonl ,  polygon2_pt ) ; 

}while  (not (visible(polygonl_pt,  polygon2_pt ) ; 

/♦expansion  pt  tangent  is  plus 
plus  tangent  to  polygon  2*/ 
do  { 

polygon2_pt  =  plus_tangent(polygon2,  polygonl_pt ) ; 
polygonl_pt  =  minus_tangent (polygonl,  polygon2_pt ) ; 

)while  (not (visible (polygonl_pt,  polygon2_pt ) ; 

/♦expansion  pt  tangent  is  minus 
minus  tangent  to  polygon  2*/ 
do  { 

polygon2_pt  =  minus_tangent (polygon2 ,  polygonl_pt ) ; 
polygonl_pt  =  minus_tangent (polygonl,  polygon2_pt ) ; 

}while  (not (visible (polygonl_pt,  polygon2_pt ) ; 


h.  Vertical  Tangents 

Compared  to  finding  the  plus  and  minus  tangents, 
finding  the  vertical  tangents  is  a  relatively  simple 
exercise.  Since  I  only  am  concerned  about  the  top  vertical 
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tangent  of  each  polyhedron,  I  refer  to  this  tangent  as  the 


vertical  tangent.  Figure  11  depicts  the  vertical  plane, 
the  obstacle,  and  the  tangent. 


Figure  11  -  Vertical  Tangent 

Unlike  the  functions  plus_tangent  and 
minus_tangent ,  which  find  a  point  which  forms  the  tangent, 
the  process  I  developed  finds  not  only  that  one  point  which 
forms  the  tangent,  but  it  also  finds  the  intersection  point 
of  the  vertical  plane  which  is  opposite  to  the  side  of  the 
top  polyhedron  face  from  which  came  the  first  point.  Figure 
12  depicts  the  scenario.  I  include  both  intersection  points 
b''.cause  if  the  vertical  extension  of  a  path  is  to  go  over 
only  one  polyhedron,  then  the  second  point  will  eventually 
be  needed.  Since  the  vertical  plane  is  already  determined, 
then  it  is  logical  to  expand  the  path  to  the  second  point 
and  not  the  first.  In  addition,  the  process  finds  all 
vertical  points  to  all  polyhedrons  lying  between  the 


expansion  point  and  the  end  point.  In  the  event  that  there 
are  more  than  one  obstacle,  then  I  have  also  included  a 
check  which  determines  if  points  are  visible  not  to  the  next 
point  in  the  list,  but  to  the  other  points  in  the  vertical 
list  as  depicted  in  Figure  13 .  This  allows  me  to  exclude 
all  intersection  points  not  needed  in  developing  the 
extended  path.  Appendix  J  contains  the  source  code  for 
finding  these  vertical  points. 


One  Polygon 


More  Than  One  Polygon 


6 .  Path  Representation 

In  designing  my  algorithm  for  the  path  planning 
process,  I  had  to  develop  a  structure  which  I  could 
represent  the  path.  This  path  structure  evolved  through 
many  iterations  until  I  arrived  at  the  final  structure. 
Figure  14  shows  the  structure  and  the  file  3d_tan.h  is  in 
Appendix  L  contains  the  source  code  defining  the  structure. 
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Figure  14  -  Partial  Path  List 

The  Next_path  pointer  and  the  Last_node  pointer  in 
the  list  are  self-explanatory.  The  3  cost  entries  are  used 
in  the  search  function  to  identify  the  shortest  path  to 
expand  the  path.  The  cost  entry  is  the  cost  from  the  start 
point  traveling  along  the  path  to  the  last  node  added  to  the 
goal;  the  estimated  cost  entry  is  the  euclidean  distance 
from  the  last  point  added  to  the  goal;  and  the  total  cost  is 
the  summation  of  the  other  two  costs ,  The  ray  direction 
entry  is  the  direction  on  the  X  Y  plane  of  the  tangent  from 
the  second  to  last  node  to  the  last  node  added  to  path. 

This  entry  is  used  in  a  direction  heuristic,  which  is  used 
to  prune  possible  path  entries.  The  type_tangent  entry  is 
the  type  of  the  last  tangent  formed  by  the  last  node  and  the 
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second- to-last  node.  The  entry  "Plane  number"  is  the  number 
of  the  horizontal  plane  slice  used  in  finding  the  path 
around  obstacles .  This  entry  ensures  that  the  same  plane  is 
used  to  expand  a  path  around  the  obstacles.  The  able2expand 
entry  is  a  flag  used  by  one  of  the  heuristic  to  determine  if 
the  path  should  be  extended  or  not.  The  integer  forward 
identifies  the  path  as  built  from  the  start  to  goal  or  from 
the  goal  to  the  start.  Polyhed4vert  identifies  the  polygon 
which  the  path  will  go  around  after  going  over  the  preceding 
obstacles .  The  Polyhed4vert_PTR  is  a  pointer  which  points 
to  an  X  y  node  structure  and  is  used  to  expand  the  path  from 
a  vertical  node  around  an  obstacle  and  over  other  obstacles 
(path  types  5  and  6).  I  refer  to  the  information  in  this 
structure  as  the  header  information  of  the  path. 

7 .  Active  Node  List  and  Active  Paths  Heuristic 

The  Active  Node  List  is  a  link  list  which  identifies 
all  those  nodes  which  have  been  added  to  the  paths  in  the 
partial  path  list.  Figure  15  shows  the  structure  I  use  in 
this  link  list.  As  with  the  partial  path  structure,  the 
source  code  for  this  structure  is  found  in  3d_tan.h  in 
Appendix  L.  Along  with  the  X  Y  Z  point,  the  structure  also 
has  an  entry  for  the  cost  of  the  path  from  start  to  this  X  Y 
Z  point  and  an  integer  which  corresponds  to  when  the  path 
was  found. 
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Figure  15  -  Active  Node  List 


This  linked  list  is  used  in  a  heuristic  which 
determines  if  a  path  being  expanded  from  has  been  visited  or 
not.  If  the  point  has  already  been  visited,  the  distance 
entry  in  the  structure  is  compared  with  the  distance  from 
the  start  to  the  last  node  in  the  expanded  path  plus  the 
distance  from  the  last  node  to  the  X  Y  Z  point.  If  the 
distance  in  the  structure  of  the  link  list  is  less  than  the 
computed  distance,  then  the  new  point  will  not  be  added  to 
the  link  list  as  an  active  node  and  the  path  will  not  extend 
to  the  this  point.  If  the  distance  in  the  list  is  greater 
than  the  computed  distance,  then  the  new  point  is  added  to 
the  link  list  as  an  active  node  and  the  path  will  be 
extended  to  the  new  point.  In  addition,  the  partial  path 
with  which  the  entry,  when_path_found,  equals  the  entry  in 
the  list,  then  the  entry,  able2expand,  is  marked  as  not 
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extendable.  Also  the  distance  and  when_path_found  entries 
are  updated  to  reflect  the  data  for  this  new  added  node  to 
the  partial  path.  If  the  node  has  not  been  visited  in  the 
path  planning  process,  the  node  with  its  corresponding  data 
is  added  to  the  active  node  list  and  the  partial  path  is 
extended  to  that  point.  Table  XIII  depicts  the  pseudo 
language  for  this  heuristic  and  the  source  code  is  in 
Appendix  D. 


Table  XIII  -  CHECK  ACTIVE  NODE  PSEUDO  LANGUAGE 


check_active_node_list (node_list,  shortest_path,  pt, 

new_when_path_f ound ) 

{ 

for ( each  active_node ) { 

if (active_node->pt  ==  shortest_path->last_node) { 
if (active_node->distance  >  shortest_path->cost  + 
distance(shortest_path->last_node,  pt) { 
mark_old_path_unexpandable(when_path_found) ; 
update_active_node ( new_distance , 

new_when_path_found) ; 
return ( EXPAND ) ;  /*EXPAND  ==  1*/ 

} 

else  return ( NOT_EXPANDABLE ) ; 

) 

) 

/♦node  not  visited  yet*/ 

insert_node_into_node_list(node_list ,  pt,  cost); 
return (EXPAND) ; 


8 .  Visibility 

After  the  algorithm  has  found  the  shortest  path  to 
extend,  the  algorithm  first  determine  if  the  last  node  added 
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to  the  path  is  visible  either  the  goal  or  start  point.  This 
case  is  different  from  the  visibility  check  used  for  the 
start  and  goal  points  because  the  last  node  can  be  on  the 
backside  of  a  polyhedron  from  the  goal  and  still  be  visible 
with  the  goal  as  long  as  other  polyhedrons  lie  between  the 
last  node  and  the  goal.  Figure  16  depicts  this  case.  The 
concept  of  finding  the  path  around  the  obstacle  appears 
relatively  simple;  however  the  code  is  not.  Table  XIV  lists 
the  pseudo  language  I  developed  to  solve  this  problem. 

Figure  17  identifies  the  points  associated  with  both 
tangents  of  the  situation  shown  in  Figure  16. 


Figure  16  -  Last  Node  Goal 

Visibility 


Figure  17  -  Points  found  for 
both  Tangents 
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Table  XIV  -  PSEUDO  LANGUAGE  FOR  VISIBILITY 


Visibility (expand_point,  goal_point,  start,  two_d_list, 
shortest_path ) 
i 

if (shortest_path  forward) 
goal  =  start; 
else  goal  =  goal_point; 

if ( I intersect_polyhedron ( expand_point ,  goal ) ) { 
add_goal_to_goal_list ; 
return (goal_l is t) ; 

if (shortest_path->type_tangent  =  VERTICAL) 
return (NULL) ; 
else{ 

f ind_polygon_las t_node_is_on ; 

/♦finds  the  plus  tangent  from  goal  to  polygon 
for (each  type  of  tangent  from  goal  to  the  polygon) 
if ( PLUS_TANGENT ) 

from_polygon_pt  = 

plus_tangent(polygon->polygon_start,  goal) ; 

else 

from_polygon_pt  = 

minus_tangent (polygon ->polygon_s tart,  goal) ; 

/♦checks  for  intersection^/ 

if ( intersect_polyhedron ( f rom_polygon_pt ,  goal ) ) 
return(NULL); 

else{  /♦two  points  are  visible^/ 

place  goal  and  from  polygon  pt  in  goal  list 
find  from_polygon_pt  on  polygon 
if ( PLUS_TANGENT ) { 

cycle  counter-clockwise 

add  points  to  goal  list  until  arrive  at 
expand_point 

} 

else{ 

cycle  clockwise,  add  points  to  goal  list 
until  arrive  at  expand  point 

} 

if ( direct ion_heuristic(shortest_path, 

goal_list->pt ) 

return ( goal_list ) ; 

} 

} 

deallocate  and  free  memory  of  goal  list 
return ( NULL ) ; 
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In  the  pseudo  language  for  the  function  visible,  I 
refer  to  a  function  called  " direct ion_heuristic" .  This 
function  uses  geometric  concepts  of  polygons  and  rays  to 
eliminate  a  path  from  extending  incorrectly.  If  Figure  17 
is  used  as  an  example,  the  " direct ion_heuristic"  ensures 
that  the  nodes  associated  with  the  minus  tangent  are  not 
chosen,  while  the  nodes  associated  with  the  plus  tangent. 
Figure  18  depicts  why  points  F  and  G  would  not  be  chosen 
since  point  B  can  extend  directly  to  G.  Appendix  F  contains 
the  source  code  for  the  "direction  heuristic"  function. 


Figure  18  -  Direction  Heuristic 
Characteristics 


The  "visibility"  function  returns  to  the  main 
function  the  goal  list.  If  the  goal  list  is  not  NULL,  then 
the  while  loop  is  exited  and  the  algorithm  will  add  the  goal 
list  to  the  shortest  path,  update  the  header  information  of 
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the  shortest  path,  delete  all  other  paths,  and  terminate. 

If  the  goal  list  is  NULL,  then  the  algorithm  enters  the  loop 
structure  to  begin  extending  the  best  path  chosen  to  extend. 
Prior  to  extending  the  path,  the  algorithm  prints  to  a  file 
or  to  the  screen  the  paths  that  are  in  the  partial  path  list 
and  the  path  chosen  as  the  shortest  path  to  extend. 

B.  THREE-DIMENSIONAL  PATH  PLANNING 

As  stated  above,  my  three-dimensional  path  planning 
follows  along  the  same  concepts  as  Kanayama's  two- 
dimensional  algorithm.  The  basic  algorithm  begins  by 
prompting  the  user  to  input  the  start  and  goal  points .  Once 
the  user  inputs  these  points,  the  algorithm  checks  whether 
or  not  there  are  obstacles  lying  between  the  two  points . 

If  there  are  one  or  more  obstacle  lying  between  the  start 
point  and  goal,  the  algorithm  then  finds  the  two  dimensional 
representation  of  the  environment  using  the  horizontal  plane 
formed  by  the  start  and  goal  points  as  discussed  earlier  in 
this  chapter.  After  finding  this  two-dimensional 
representation,  the  algorithm  begins  to  find  all  the  initial 
partial  paths.  After  the  algorithm  finds  these  initial 
paths,  it  searches  the  path  list  to  find  the  path  to  expand 
from  after  the  initial  paths  are  found.  Next,  the  algorithm 
checks  to  determine  if  either  the  goal  for  a  forward  type 
path  or  the  start  point  for  a  reverse  path  is  visible  from 
the  last  node  added  to  the  partial  path.  If  it  is,  the  goal 
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is  added  to  that  path,  the  path  is  marked  as  a  completed 
path,  all  partial  paths  are  deleted  and  associated  memory 
deallocated,  and  the  process  ends.  If  the  goal  or  start 
point  is  not  visible,  then  the  path  is  extended  to  all  valid 
points  in  the  environment  and  the  process  begins  again  with 
searching  for  a  new  path  to  expand.  Table  XV  shows  this 
basic  algorithm  in  pseudo  language. 


Table  XV  -  PATH  PLANNER  PSEUDO  LANGUAGE 


Three-dimensional_path_planner ( ) 

{ 

Create_3D_world( ) ; 

Input_start_and_goal ( ) ; 

if( ! intersect_polyhedron( start,  goal)){ 
build_best_path ( start_goal ) ; 
return ( best_path ) ; 

} 

Two__D  =  build_two_dimensional_representat ion  (start, 

goal ) ; 

Partial_paths  =  Find_initial_paths ( 3D_world,  Two_D, 

start,  goal); 

while ( visible ( f ind_shortest_path ( part ial_paths ) , 
start_point ,  goal ) ) { 

expand_shortest_path(shortest_path,  3D_world, 

Two_D,  goal , active_node_list ) ; 

) 

Add_goal_and_mark_completed ( shortest_path ) ; 
Delete_partial_paths (partial_paths ) ; 


1 .  Initial  Paths 

In  finding  the  initial  paths,  the  main  algorithm  or 
the  function  "main (void)  calls  the  functions 
"start_f inding_paths" .  This  function  accepts  the  polygon 
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list,  the  start  point  and  the  goal  point  and  returns  to  the 
main  function  the  initial  partial  paths.  Table  XVI  shows 
the  pseudo  language  I  developed  for  the  function.  Appendix 
F  contains  the  source  code  for  this  function. 

Table  XVI  -  PSEUDO  LANGUAGE  FOR  START  FINDING  PATHS 


start_f inding_paths ( 3d_world,  polygons,  start,  goal) 

{ 

f ind_vertical_and_hori2ontal_planes 

polygons  =  build_2d_polygon_world ( 3d_world , 
horizontal ) 

for (each  polygon  in  list){ 

ptl  =  plus_tangent(polygon_nodes,  start); 
pt2  =  minus_tangent (polygon_nodes ,  start); 
if (direction_heuristic_from_start (ptl,  start,  goal){ 
if (visible(ptl,  start) 

create_new_partial_path(partial_paths ) ; 
add_start_goal (start,  goal,  new_path) 
update_header ( new_path,  goal ) ; 
else 

partial_paths  =  up_and_over (start_point , 

3d_world ,  ptl ) ; 

if (direction_heuristic_from_start(pt2,  start,  goal){ 
if (visible(ptl,  start) 

create_new_partial_path(partial_paths ) ; 
add_start_goal (start ,  goal,  new_path) 
update_header (new_path,  goal); 
else 

partial_paths  =  up_and_over ( start_point , 

3d_world,  pt2 ) ; 

end  for  loop 

partial_paths  =  up_and_over (start ,  goal,  3d_world); 


The  function  "start_f inding_paths "  begins  by  finding 
the  vertical  ana  horizontal  planes  formed  by  the  start  and 
goal.  It  then  proceeds  to  find  the  polygons  formed  by  the 
intersection  of  this  horizontal  plane  and  the  polyhedrons  in 
the  world.  After  finding  this  polygon  list,  the  process 
starts  with  uhe  first  polygon  in  the  list  and  finds  both  the 
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plus  and  minus  tangent  points  with  respect  to  the  start 
point  as  described  earlier  in  this  chapter.  The  process 
then  uses  a  heuristic  to  determine  if  the  direction  of  the 
ray  formed  by  the  start  and  goal  is  within  plus  or  minus  180 
degrees  of  the  ray  formed  by  the  start  a^.d  goal  points.  If 
this  heuristic  determines  that  the  ray  is  valid,  then  the 
process  continues  by  making  additional  checks.  If  the 
heuristic  determines  that  the  ray  is  not  valid,  the  process 
stops  checking  the  plus  tangent  point  and  begins  the  process 
over  checking  the  minus  tangent  point. 

Once  the  process  determines  if  the  ray  is  valid,  it 
begins  to  check  if  the  start  point  is  visible  with  this  plus 
tangent  point.  The  process  uses  the  polygon  list  to 
determine  if  the  two  points  are  visible.  If  a  two  points 
are  not  visible  using  this  two-dimensional  polygon  list, 
then  the  two  points  will  not  be  visible  using  polyhedron 
list  since  the  polygon  list  is  a  subset  of  the  polyhedron 
world.  By  using  this  smaller  polygon  list,  the  process  can 
check  for  visibility  faster  than  if  it  used  the  polyhedron 
list . 

In  determining  that  the  two  points  are  visible,  the 
process  begins  to  find  a  partial  path  which  corresponds  to 
the  tvpe  of  path  in  which  all  nodes  are  on  the  same  plane. 

If  the  two  points  are  visible  then  the  process  adds  a  new 
partial  path  to  the  partial  path  list.  This  partial  path 
list  has  the  start  point  as  the  first  point  and  the  plus 
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tangent  point  as  the  last_node.  In  addition,  the  process 
also  computes  the  header  data  of  the  partial  path. 

If  the  process  determines  that  the  two  points  are 
not  visible,  then  the  process  begins  the  phase  of  finding 
the  type  of  path  which  goes  over  one  or  more  polyhedrons  and 
then  around  (at  least  one)  the  rest  of  the  polyhedrons  to 
the  goal . 

In  finding  the  this  path,  the  process  calls  the 
function  " up_and_over " ,  passing  to  the  function  the  partial 
paths  pointer,  expansion  point,  start  point  and  the  either 
the  plus  or  minus  tangent  point,  depending  on  which  point  is 
being  checked.  The  function  returns  the  partial  path  list 
adding  to  the  list  the  vertical  path  as  described  earlier  in 
this  chapter.  The  function  up_and_over  and  the  functions 
this  function  calls  can  be  found  in  Appendix  J  for  further 
analysis . 

After  each  polygon  in  the  two-dimensional  list  has 
been  checked,  the  "start_finding_f unction"  then  finds  the 
partial  path  which  travels  over  all  the  polyhedrons  lying 
between  the  start  and  goal.  It  does  so  by  using  the  same 
function  used  above.  In  using  the  "up_and_over "  function, 
the  calling  function  passes  the  same  parameters,  except  the 
goal  point  is  passed  instead  one  of  the  two  tangent  points. 

The  last  step  the  process  determines  is  to  find  the 
paths  which  go  around  one  or  more  polyhedrons  and  then  over 
(at  least  one)  the  remaining  polyhedrons.  This  step  is  not 
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as  difficult  as  it  appears.  Upon  analyzing  this  step,  I 
noticed  that  this  step  was  the  similar  to  the  step  of 
finding  all  the  paths  which  go  over  one  or  more  polyhedrons 
and  around  the  remaining  only  in  reverse  order.  As  a 
result,  to  find  these  type  of  paths,  I  developed  a  sub¬ 
process  which  extends  from  the  goal  and  not  the  start  point. 

The  "start_f inding_paths "  function  calls  the 
function  ”up_and_over_from_goal" .  I  developed  this  function 
along  the  same  lines  as  "start_f inding_paths " .  For  each 
polygon  in  the  two-dimensional  list,  the  sub-process  finds 
both  tangents  points  with  respect  to  the  goal,  the  expansion 
point.  After  finding  the  these  points,  the  sub-process 
first  determines  if  the  expansion  point  is  visible  to  the 
plus  tangent  point.  If  the  points  are  not  visible,  the 
"up_and_over "  function  is  called,  adding  paths  extended  from 
the  goal.  After  checking  the  plus  tangent  point,  the  sub¬ 
process  then  performs  the  same  operations  for  the  minus 
tangent  point.  To  distinguish  these  paths  from  the  paths 
extended  from  the  start  point,  the  "forward"  integer  flag  in 
the  header  information  of  the  path  is  flagged  equal  to  0 
while  this  flag  is  equal  to  1  if  the  path  extends  from  the 
start.  Upon  returning  control  back  to 

"start_f inding_paths" ,  the  function  returns  the  partial  path 
list,  which  in  turn  returns  the  same  list  back  to  the  main 
function.  As  with  the  other  vertical  function,  the 
"up_and_over_from_goal"  function  is  listed  in  Appendix  J. 
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When  control  of  the  process  returns  to  the  main 
function,  all  the  partial  paths  which  are  needed  to  find  the 
best  path  are  included  in  the  list.  From  this  list,  the 
process  searches  for  the  best  path  from  which  to  extend 
from. 

2 .  Extending  Partial  Paths 

In  Extending  a  partial  path,  the  algorithm  enters  a 
while  loop  structure  which  first  searches  the  partial  path 
list  to  find  the  best  path  to  extend,  secondly  determines 
the  visibility  of  the  last  node  of  the  best  path  chosen,  and 
thirdly,  based  on  the  results  of  checking  the  visibility, 
extends  this  path  or  exits  the  loop, 
a .  Search  Technique 

After  finding  these  initial  paths,  the  process 
uses  an  A*  (A  Star)  search  to  determine  which  path  to 
extend.  When  considering  the  various  paths  for  expansion, 
the  A*  search  uses  the  function  f(n)  =  g(n)  +  h(n)  where 
g(n)  is  the  actual  cost  of  the  path  from  either  the  start  if 
a  forward  path  or  the  goal  if  a  reverse  path;  h(n)  is  the 
heuristic  estimate  or  the  cost/distance  from  either  the  last 
node  to  the  goal  if  the  path  is  a  forward  path  or  the  last 
node  to  the  start  point  if  the  path  is  a  reverse  path.  In 
the  header  data  of  each  path  g(n)  is  the  "cost"  entry;  h(n) 
is  the  "estimated  cost"  entry;  and  f(n)  is  the  "total  cost" 
entry.  The  minimum  f(n)  of  all  possible  paths  identifies 
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the  shortest  partial  path  and  determines  which  path  the 
algorithm  will  use  to  extend  to  the  next  set  of  nodes . 

The  advantage  of  using  the  A*  Search  is  that  the 
heuristic  measure,  h(n)  is  a  lower  bound  of  the  actual  cost 
of  the  shortest  path.  As  a  result,  the  algorithm  is 
admissible,  finding  an  optimal  path  if  one  exists.  Once  the 
A*  Search  has  determined  which  path  to  extend,  the  algorithm 
begins  a  process  similar  to  the  one  used  to  find  the  initial 
paths . 

b.  Extending  the  Shortest  Path 

Extending  the  shortest  path  begins  with  the 
function  "extend_path" .  The  main  function  passes  to 
"extend_path"  the  following  parameters:  start  point;  goal; 
and  pointers  to  the  shortest  path,  polygon  list,  polyhedron 
list  and  the  active  node  list.  It  returns  to  the  main 
function  the  pointer  to  the  shortest  path. 

Table  XVII  lists  the  pseudo  language  I  developed 
to  extend  the  path.  As  can  be  seen,  the  concept  behind  the 
function  is  to  check  the  type  of  tangent  entry  in  the  header 
information  of  the  shortest  path.  If  the  type  tangent  is 
not  a  vertical  tangent,  then  the  path  is  extended  on  the 
plane  slice  indicated  by  the  plane  number  entry  in  the 
header  information.  However,  if  the  type_tangent  entry  is 
vertical,  then  a  new  plane  slice  must  be  used.  In  addition, 
the  function  is  Appendix  F. 
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Table  XVII  -  PSEUDO  LANGUAGE  FOR  EXTEND  PATH 


extend_path 

{ 

if (shortest_path->type_tangent  !=  VERTICAL) 
f ind_tangents_f rom_inner_nodes ; 
else{ 

add_on_new_plane_intersection ; 
continue_vertical_path; 

} 

return ( shortest_path ) ; 

} 

c.  Vertical  Extension 

In  extending  the  path  in  which  the  tangent  to  the 
last  node  of  the  path  is  a  vertical  tangent,  the  process 
first  finds  the  horizontal  plane  formed  by  the  last  node  of 
the  path  and  the  goal.  Then  the  process  finds  the 
intersection  of  the  plane  with  each  polyhedron  in  the  world. 
These  new  polygons  are  appended  to  the  list  of  the  existing 
polygons  with  the  plane_number  entry  corresponding  to  the 
numtar  of  horizontal  planes  used  finding  polygon 
intersections . 

After  finding  the  polygon  intersections  for 
the  new  horizontal  plane  slice,  the  process  must  accomplish 
two  tasks.  The  first  task  is  the  find  the  type  of  paths 
which  correspond  to  the  last  two  types  of  paths  as  referred 
to  earlier  in  the  chapter.  The  second  task  is  to  extend  the 
path  to  the  one  node  on  the  horizontal  plane  slice  found 
directly  before  beginning  this  process. 
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To  find  the  paths  which  begin  traversing  over 
one  or  more  obstacles,  then  around  one  or  more  obstacles  and 
then  over  at  least  one  obstacle  or  similarly  around,  over 
and  then  around  are  found  from  a  vertical  point.  As  with 
the  paths  which  correspond  to  the  third  and  fourth  types  of 
depending  on  the  direction  of  the  path.  If  the  direction  of 
the  path  is  forward,  then  the  type  of  path  found  is  up, 
around  and  up;  however,  if  the  direction  is  from  the  goal, 
the  type  of  path  is  around,  up,  and  around.  Since  finding 
these  two  types  of  paths  are  the  hardest  to  find,  I  left 
them  for  the  last  task  to  accomplish  and  at  the  time  of 
writing  of  this  paper  I  have  not  implemented  the  process 
into  the  algorithm  which  I  developed.  However,  Table  XVIII 
shows  the  process  written  in  pseudo  language. 
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Table  XVIII  -  PSEUDO  LANGUAGE  FOR  FUNCTION  UP,  OVER,  UP 


up_over_up ( path ) 

{ 

For ( each  polyhedron ) { 

from  =  find  top  face(lst_polyhedron) ; 
for (each  polyhedron  not  the  1st  polyhedron ){ 
to  =  find  top  face  (2nd_polyhedron) ; 
for (loop  4  times)  /*there  are  4  tangents  to 

find*/ 

tangent_list  =  tangent(to,  from,  model,  mode2); 
if ( check_tangent_list ( tangent_list ) { 
f ree_tangent_list ( tangent_list ) 
continue; 

) 

vertical_list  =  find_vertical_list (tangent_ptl, 
tangent_pt2 ) ; 

last_node  =  find  last  node  in  vertical_list ; 
new_poly_list  =  build  two-d  polygon  list 

(last_node,  path's 
last_node ) 

new_path_list  =  duplicate_path(path) 
new_path_list  =  continue_vertical_path  using 
new_poly_list 
while(not (visible ( 

f ind_shortest_path(new_paths_list ) , 
new_poly_list,  tangent_Ptl) 
new_path_list  =  extend_path(new_path_list , 

tangent_ptl , newjpoly_list ) ; 
/*tangent_ptl  is  new  goal  point*/ 

} 

add  tangent_ptl  to  shortest  path; 
using  this  process'  shortest  path 

if  (direction  heuristic  with  last  node  in  path 
and  last  node  in  vertical_path) 
add  vertical  list  to  shortest  path 
else  mark  path  as  not  able2expand 
delete  all  partial  paths  in  new  list  except 
shortest 

place  in  locally  shortest_path_list 
f ree_new_poly_list ( new_poly_list ) ; 

1 

1 

add  locally  shortest  path  list  to  partial  path  list 
after  passed  in  path  and  connect  links 
return (path) ; 

} 


In  beginning  the  second  task  of  the  process,  the 
process  uses  these  new  polygons  and  finds  the  one  node  which 
extends  the  path  along  the  same  ray  direction  in  the  header 
information.  In  doing  so,  the  process  scans  these  new 
polygons  checking  the  polyhedron  entry.  If  this  entry 
equals  the  entry  of  polyhed4vert,  then  the  process  finds  the 
two  horizontal  tangents  from  the  last  node  to  the  polygon. 
The  process  then  determines  the  rays  of  these  two  tangents . 
The  process  adds  to  the  path  as  the  last  node  the  tangent 
point  whose  tangent  direction  is  in  the  same  general 
direction  as  the  ray_direction  entry  in  the  header 
information . 

In  determining  if  the  two  rays  are  in  the 
same  general  direction,  I  had  to  consider  the  case  where  the 
first  plane  slice  created  a  polygon  with  x  and  y  entries  in 
the  link  list  that  are  different  from  the  x  and  y  entries  in 
the  link  list  of  the  polygon  formed  by  the  new  plane  slice. 
Since  the  first  plane  and  its  associated  polygons  were  used 
to  determine  the  vertical  points  of  the  path,  the  new 
polygon  used  to  determine  the  tangent  point  will  result  in  a 
different  path  if  this  tangent  point  and  start  point  are 
used. 

In  determining  if  the  point  should  be  added, 

I  modified  the  process  to  first  check  the  type_tangent  entry 
in  the  structure  which  the  pointer  polyhed4vert_PTR  is 
linked  to.  The  process  then  executes  the  correct  tangent 
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function  (either  plus_tangent  or  minus_tangent ) .  The  next 
check  the  process  does  is  to  determine  if  the  difference  in 
ray  directions  is  less  than  .0001.  Instead  of  using  zero,  I 
use  this  value  since  this  algorithm  may  be  used  on  different 
platforms  and  the  precision  of  the  platforms  may  be 
different.  If  the  difference  is,  then  the  point  is  added  to 
the  list.  If  the  difference  is  not,  then  the  process  must 
recalculate  the  vertical  intersection  points  using  the 
vertical  plane  formed  by  the  start  point  and  this  new  point. 
In  both  cases,  the  header  information  is  updated  accordingly 
and  the  function  returns  to  calling  function,  extend_path, 
the  shortest  path.  The  function  "continue_vertical_path"  is 
in  Appendix  J. 

(1)  Horizontal  Extension 

If  the  function  ''extend_path"  determines  that 
the  shortest_path' s  type_tangent  entry  does  not  indicate  a 
vertical  tangent,  then  the  process  begins  to  find  all  valid 
tangents  on  the  plane  slice  indicated  by  the  plane_number 
entry  in  the  header  information.  The  function  which 
performs  this  work  is  "f ind_tangents_from_inner_nodes "  and 
the  source  code  is  in  Appendix  F. 

The  main  concept  behind  this  function  is  to 
cycle  through  the  polygon  list.  For  those  polygons  which 
have  the  same  plane  number  as  the  shortest  path's 
plane_number  entry,  the  tangent  formed  by  the  polygon  being 
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checked  and  the  polygon  which  the  last  node  of  the  shortest 
path  lies  as  described  earlier  in  this  chapter. 

The  process  continues  on  to  check  whether  the 
ray  formed  by  the  two  tangent  points  intersects  any  of  the 
polyhedrons  in  the  world.  If  there  is  not,  then  the 
shortest  path  is  duplicated  entirely,  the  two  tangent  points 
are  added  to  the  duplicated  path.  In  addition,  all  points 
which  lie  on  the  same  polygon  as,  are  between  the  shortest 
path's  last  node  and  one  of  the  tangent  points,  and  are  in 
the  direction  (either  plus  or  minus)  the  path  is  advancing 
are  added  to  the  path  between  these  two  points.  After 
completing  the  duplication  and  insertion  of  the  new  points, 
the  header  information  is  updated  and  the  process  inserts 
the  new  partial  path  into  the  partial  path  list  after  the 
shortest  path,  reconnecting  the  link  list  accordingly. 

C.  FINDINGS  AND  EXAMPLES 

As  I  have  already  indicated,  the  number  of  paths  grows 
in  polynomial  time  and  is  dependent  not  only  on  the  number 
of  polyhedrons  but  also  the  number  of  faces  in  each 
polyhedron  and  number  of  nodes  in  each  face.  As  a  result,  I 
will  limit  the  number  of  polyhedrons  in  the  environment  to  1 
and  2.  In  addition,  I  have  included  a  print  function  which 
prints  out  each  partial  path  listing  prior  to  an  iteration 
of  extending  the  shortest  path.  A  listing  of  for  each 
example  is  in  Appendix  M. 
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1.  One  Polyhedron  World 

For  the  1  polyhedron  world,  I  use  the  polyhedron 
described  in  Table  XIX.  In  the  first  example,  I  use  the 
starting  point  (1,  15,  14)  and  the  goal  point  (40,  15,  14). 

I  use  the  same  depth  value  (z  value)  in  the  example  to  show 
that  the  plane  slice  does  correctly  work.  Figure  19  depicts 
the  scenario  with  a  top  down  view  for  this  example. 

In  generating  the  initial  partial  path  list,  the 
algorithm  calculated  3  partial  paths.  Using  the  same  top 
down  view  of  Figure  19,  Figure  20  shows  the  partial  paths 
with  nodes  of  the  path  labeled. 

The  reader  should  notice  that  the  nodes  of  the  path 
which  end  at  a  vertex  of  the  polygon  in  Figure  20  are  nodes 
of  the  polygon  found  using  the  horizontal  plane  formed  by 
the  start  and  goal  points.  These  two  paths  also  represent 
the  type  of  paths  in  which  all  nodes  of  the  path  lie  on  the 
same  plane.  Similarly,  the  one  path  in  Figure  20  which  does 
not  have  a  tangent  to  the  polygon  as  a  node  of  the  path 
represents  the  path  which  traverses  over  all  polyhedrons  to 
the  goal.  In  addition,  the  intersection  points  in  Figure  20 
represent  the  intersection  of  the  vertical  plane  and  the  top 
face  of  the  polyhedron. 
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Table  XIX  -  ONE  POLYHEDRON  WORLD 


SIDE  FACE  1:  (10  7  15)  (10  20  15)  (10  20  3)  (10  7  3) 

SIDE  FACE  2:  (10  20  15)  (14  20  15)  (14  20  3)  (10  20  3) 

SIDE  FACE  3:  (14  20  15)  (14  7  15)  (14  7  3)  (14  20  3) 

SIDE  FACE  4:  (14  7  15)  (10  7  15)  (10  7  3)  (14  7  3) 

TOP  FACE:  (10  7  3)  (10  20  3)  (14  20  3)  (14  7  3) 

BOTTOM  FACE:  (10  7  15)  (10  20  15)  (14  20  15)  (14  7  15) 


The  algorithm  chose  the  path  (1  15  14)  to  (10  20  14) 
to  extend  using  the  A*  search.  In  checking  the  visibility 
of  the  point  with  the  goal,  the  algorithm  determined  that 
the  expansion  point  was  visible.  As  a  result  the  correct 
node(s)  of  the  polygon  were  added  to  the  path  along  with  the 
goal,  the  header  information  of  the  path  updated,  all  other 


paths,  the  polygon  list  and  the  polyhedron  list  are  deleted 
from  memory  and  the  algorithm  terminates  with  the  best  path 
as  (1  15  14)  to  (7  20  14)  to  (14  20  14)  to  (40  15  14). 

In  the  second  example  using  the  one  polyhedron 
world,  I  used  ^he  points  (1  13  5)  and  (  1  40  14)  for  the 
start  and  goal  points  respectively.  From  the  top  down  view, 
these  initial  partial  paths  look  similar  to  the  paths  in  the 
previous  example;  however,  since  the  start  and  goal  points 
are  not  at  the  same  depth,  the  intersection  of  the 
polyhedron  and  the  horizontal  plane  produces  a  polygon  with 
different  depth  values  than  the  first  example  as  shown  in 
Figure  21. 


1  Polyhedron 

In  this  example,  as  expected,  rhe  shortest  path  is 
the  vertical  path.  Since  the  vertical  path  is  directly 
visible  to  the  goal,  the  algorithm  adds  the  goal  to  the  path 
list  and  again,  it  performs  all  the  administrative  functions 
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to  update  the  header  information,  deletes  all  appropriate 
paths  and  terminates . 

2 .  Two  Polyhedron  World 

For  the  2  polyhedron  world,  I  use  the  same 
polyhedron  as  in  the  examples  for  a  one  world  polyhedron . 
Table  XX  lists  the  coordinates  of  the  nodes  of  the  second 
polyhedron.  In  addition,  I  use  the  same  start  and  goal 
points  in  both  examples,  similar  to  the  first  two  examples. 
Figure  22  shows  the  initial  partial  paths.  As  can  be  seen 
and  expected,  the  number  of  paths  has  increased  with  the 
process  including  of  the  vertical  paths  over  the  first 
polyhedron.  In  addition,  since  the  second  polyhedron  is 
taller  than  the  first  polyhedron  (z  values  are  smaller,  thus 
at  a  shallower  depth).  Figure  22  also  shows  those  nodes  of  a 
path  marked  with  an  "x” .  With  respect  to  the  one  vertical 
path  from  start  to  goal,  close  observation  will  show  that 
the  one  node  on  the  edge  of  the  top  face  closest  to  the 
second  polyhedron  was  not  included  in  the  path.  This  node 
is  not  needed  since  the  nodes  in  between  this  node  are 
visible  to  each  other.  As  in  the  first  example,  the  process 
chose  the  partial  path  (1  15  14)  to  (10  20  14)  as  the  path 
to  extend.  Figure  23  shows  only  those  partial  paths  which 
are  generated  from  extending  the  shortest  pat.c.  Since  there 
are  obstacles  between  the  last  node  of  the  path  and  the 
goal,  the  process  chooses  the  partial  path  (1  15  14)  to  (10 
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20  14)  to  (14  20  14)  to  (20  20  14)  for  the  next  iteration  of 
extending  the  shortest  path.  This  partial  path  is  visible 
to  the  goal  so  the  algorithm  as  before,  performs  the 
administrative  functions  and  terminates. 


Table  XX  -  SECOND  POLYHEDRAL  LISTING 
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Figure  23  -  Extending  a 
Shortest  Path  in  a  Two 
Polyhedral  World 


The  second  example  using  a  two  polyhedral  world  has 
the  points  (7  13  7)  and  (40  15  14)  as  the  start  and  goal 
points.  Figure  24  shows  those  paths  generated  from  the 
start.  As  ir  the  last  example,  the  second  polyhedral 
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obstacle  is  taller  than  the  other  two  polyhedral  obstacles. 
As  a  result,  the  vertical  paths  which  extend  to  this 
obstacle  will  have  only  one  intersecting  point  on  the  first 
obstacle  the  path  intersects.  Again,  the  nodes  of  the  path 
are  displayed  with  an  "x"  at  the  intersection  point.  After 
developing  these  initial  paths,  the  algorithm  iterates 
through  the  process  4  times  before  the  expansion  point  is 
visible  with  the  goal.  Figures  25,  26,  27  and  28  depict  the 
process  the  algorithm  used  to  find  the  goal. 


Two  Polyhedral  World  (Example 
2) 
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Lrd  Iterat-ion 

V.  RECOMMENDATIONS  AND  CONCLUSIONS 


The  algorithm  in  which  the  source  code  is  based  on  does 
not  end  up  finding  the  shortest  path  from  a  start  to  goal, 
but  a  best  path  for  the  two  points  for  a  point  traversing 
the  environment.  The  concept  of  three-dimensional  path 
planning  was  considerably  more  difficult  then  I  imagined. 

As  a  result,  I  was  not  able  to  implement  in  the  source  code 
the  portion  of  the  algorithm  which  solves  the  last  two  types 
of  paths  presented  in  Chapter  IV.  However,  if  implemented, 
the  shortest  path  can  be  found, 

I  initially  started  work  on  solving/finding  the  type  of 
path  which  lies  on  the  same  plane  as  the  start  and  goal 
points.  In  solving  each  of  the  other  3  remaining  types  of 
paths  which  this  thesis  addresses,  I  found  myself  developing 
again  how  I  solved  the  previously  solved  types  of  paths. 

Some  changes  were  as  simple  as  changing  the  number  of 
parameters  passed  into  the  function  or  the  structure  used  in 
passing  the  return  value  back  from  the  called  function  to 
the  calling  function.  Other  changes  included  major 
modifications . 

On  area  which  of  source  code  which  took  many  iterations 
and  time  in  forethought  was  the  header  information  structure 
for  each  path.  Every  time  I  proceeded  in  solving  the  next 
type  of  path,  I  discovered  I  needed  more  information  or  in 
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the  case  of  changing  a  function's  structure  not  needing 
specific  datum. 

This  algorithm  is  only  the  first  step  in  implementing  a 
path  planner  in  the  NPS  II  AUV.  In  addition  to  including  in 
the  source  code  the  portion  of  the  algorithm  which  solves 
the  last  two  types  of  paths,  other  modules  will  need  to  be 
added . 

One  of  the  most  important  to  me  added  is  a  module  which 
insures  that  the  path  being  extended  one  more  node  is  a  safe 
path  for  an  object  larger  than  a  point  traversing  the  world. 
Since  the  NPS  II  AUV  can  be  thought  of  as  a  cylinder 
traveling  through  the  world,  the  concept  of  safe  path 
planning  is  not  finding  the  tangential  lines  to  the 
polyhedrons  but  finding  a  cylinder  which  intersects  the 
polyhedron  at  one  point.  This  concept  is  not  as  easy  as  it 
appears.  One  particular  difficult  problem  will  be  in 
dealing  with  polyhedrons  in  which  the  sides  are  not 
perpendicular  to  the  ground. 

Other  modules  which  should  be  added  after  the  safe  path 
module  is  added  concern  implementing  the  coefficient 
characteristics  of  the  AUV  in  the  path  planner  as  well  as 
sea  and  weather  conditions  and  procedures  which  will 
aggressively  limit  the  size  of  the  world.  The  order  of 
magnitude  for  this  path  planner  is  driven  by  not  only  the 
number  of  polyhedrons,  but  also  the  number  of  nodes  in  each 
polyhedron.  By  reducing  the  search  space,  time  can  be  saved 
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in  the  planning  process .  With  regards  to  the  conditions  and 
coefficients,  they  not  only  affect  the  performance  of  the 
AUV,  but  they  also  affect  the  determination  of  a  best  path 
and  need  to  be  included. 
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APPENDIX  A 


This  appendix  contains  the  source  code  which  is  in  the  file  main.c 

tdefine  MAIN 

#include  "3d_tan.h" 
it  include  "plot.h" 


void 

( 


a 

decision ( } ; 

polyhedron_list  =  create_obstacle_list( ) ; 

boundry  =  assign_boundry_polnts ( ) ; 

printf ( "\nSTART  POINT"); 

startpolnt  =  lnput_polnt(start_point) ; 

printf ( "\nGOAL"); 

goal  *=  input_point(goal) ; 


MAIN 

- oOo- 

inain( ) 


nt 

quation 

aths 

_path 


choice,  lteration_count  =  0; 
plane_equation; 

*current_path  =  NULL; 
*goal_path  =  NULL; ; 


*/ 


inside_boundries (choice,  boundry,  start_point,  goal); 
goal_direction  -  atan2(goal.y  -  start_point . y , 
goal.x  -  start_point . X) ; 


/*  CHECK  VISIBILITY  BETWEEN  START  AND  GOAL  */ 
if  ( I intersect_polyhedron(start_point,  goal))  { 

printf ("The  best  path  to  follow  is  from  the  start  to  goal"); 
exit( 1 ) ; 

J 

/*  FIND  PARTIAL  PATHS  FROM  THE  START  TO  1ST  SET  OF  TANGENTS  */ 
start_path  =  start_finding_paths(start_path,  start_2d,  start_point,  goal, 

boundry,  head_active_nodes) ; 
print_each_path(start_path,  ++iteration_count) ; 

#lfdef  DOS 
Pause ( ) ; 

#endif 


/*  FINDS  THE  BEST  PATH  FROM  THE  INITIAL  PARTIAL  PATH  IN  THE  PATH  LIST  */ 


while  ((goal_path  =  visibillty(f ind_8hortest_path(start_path) , 

goal,  start_point,  start_2d))  == 

lf(l  I”  iteration_count) 

print_each_path(start_path,  iteration_count++ ) ; 
else 

lteratlon_count++  ; 
pr lnt_path_8hortes t ( shorte3t_path ) ; 

shorte8t_path  =  extend_path(start_point,  goal,  shortest_path, 

boundry,  head_activfc  nodes); 

J 

fprintf(fpt2,  "\n\nTHE  GOAL  HAS  BEEN  REACHED"); 


NULL)  ( 


start  2d, 
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print_each_path(start_path,  ++iteration_count) ; 
pr int_path_shor test ( shortest_pa th ) ; 

shortest_path  =  goal_reached_update(goal,  shortest_path,  goal_path); 

#ifdef  DOS 

if  (plot2screen) 

draw_shortest_path(shortest_path) ; 

#endif 

print_each_path(start_path,  ++iteration_count) ; 
pr lnt_path_8hor test ( shortest_path ) ; 

(tifdef  DOS 

if  (plot2screen)  { 
closegraph( ) ; 

1 

tendif 

fclose( fpt2 ) ; 

1 

/* . . . - . - . 

INPUT  POINT 

- oOo - 

This  function  allows  the  user  to  input  a  pt  into  a  structure  (record) 
and  returns  that  structure  to  the  calling  function 
V 

point 

input_point(pt) 

point  pt; 

1 

double  x_coord,  y_coord,  z_coord; 

printf ( "Xnlnput  x,  y,  z  coordinate:  " ) ; 

scanf("%lf  *lf  %lf'',  POINT_VALUES) ;  /*  macro  to  assign  values  to  x,y,z  */ 

pt.x  *  x_coord,  pt.y  *  y_coord,  pt.z  =  z_coord; 
return  (pt); 


/* . 

DECISION 

- - oOo - - - 

V 

int  decision( ) 

int  choice  =  0; 

char  answer; 

do( 

prlntf  (  "\nCHOOSE  ONE\nl.  IN  POOL  l\n2.  IN  POOL  2\n3  .  IN  POOL  3\n4  .  IN  POOL 

4\n-); 

prlntf ("5.  IN  POOL  5\n"); 
scanf  ( ''%d'' ,  icholce); 

/*  ESTABLISH  THE  ENVIRONMENT  */ 


switch(choice) { 
case  1: 

filename  =  " 
break; 
case  2: 

filename  *=  " 
break; 
case  3: 

filename  •=  " 
break; 
case  4 : 

filename  =  " 


pool . dat" ; 


pooll .dat" ; 


pool3.dat"; 


pool 4 .dat"  ; 
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break; 
case  5: 

filename  =  "pool2.dat"; 
break ; 
default : 

printf ( "\n\nINCORRECT  ENTRY.  Try  again"); 
clrscr ( ) ; 

) 

lwhile( Ichoice) ; 

printf ("Do  you  want  to  print  results  to  a  file  (y  or  n)?  "); 
fflush(stdin) ; 
answer  -  getc(stdin); 

if  (answer  -=  'y'  | |  answer  ==  'Y')  { 
prlnt2file  *  1; 

printf ( "\nThe  file  name  is  path.doc .\n" ) ; 

1 

else 

print2file  =  0; 
if  (print2flle)  { 

if  ((fpt2  =  f open ("path.doc",  WRITEONLY))  ==  NULL)  { 

perror("  Error:  Data  file  for  wrinting  did  not  open  correctly. \n 
exit ( 0 ) ; 

) 

1 

(tifdef  DOS 

printf ("\nDo  you  want  a  plot  of  this  path  on  the  screen  (y  or  n)?"); 
fflush(stdin) ; 
answer  ■=  getc(stdin); 
if  (answer  ==  'y'  |  |  answer  •==  'Y') 
plot2screen  =  1; 
else 

plot2screen  -  0; 

#endlf 

printf ("\nDo  you  want  a  horizontal  path  only  (y  or  n)?  "); 

fflush(stdin) ; 
answer  -  getc(stdln); 
if('y'  answer) 
horizontal  •=  1; 
else 

horizontal  =  0; 


1 
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/* 


INSIDE  BOUNDRY 

- - - qOq - 

This  function  checks  whether  or  not  the  start  point  and  goal  point/points 
are  within  the  boundries  of  the  pool.  It  calls  boundary  check  passing  to 
it  the  point  to  be  checked.*/ 


void 

int 

boundries 

point 


inslde_boundries ( choice,  boundry,  start_point,  goal) 
choice; 
boundry; 

start_point,  goal; 


{ 

int 


boolean  =  FALSE; 


If  (choice  &&  ( fboundry_check(boundry,  start_polnt) ) )  ( 

prlntf ( "\nThe  start  point  is  not  within  the  boundaries\n“ ) ; 
boolean  =  TRUE; 

1 

if  (choice  &&  ( tboundry_check( boundry ,  goal)))  ( 

printf ( "\nThe  goal  is  not  within  the  boundaries\n" ) ; 
boolean  =  TRUE; 

1 

if  (boolean) 
exit(l) ; 

1 


/* 


BOUNDRY  CHECK 


.... - - - oOo - 

This  function  determines  if  the  passed  in  point  lies  within  the 
of  the  search  space.  If  it  is  not,  a  FALSE  (0)  is  return,  else 
is  returned  to  the  calling  function.*/ 


boundries 
a  TRUE  (1) 


int  boundry_check (boundry,  pt) 

boundries  boundry; 

point  pt; 

I 

if  (pt.x  >  boundry. xl  &&  pt.x  <  boundry. x2 
pt.y  >  boundry. yl  &&  pt.y  <  boundry. y2 
pt . z  >=  boundry. zl  &&  pt.z  <  boundry. z2) 
return  (TRUE); 
else 

return  (FALSE); 
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/* 


ASSIGN  BOONDRY  POINTS 


- - qQq - 

This  function  assigns  the  boundry  points  to  the  search  area.  At  present 
it  is  fixed  at  the  pool  dimensions  used  in  the  pool.dat;  however,  at  a 
later  date,  input  function  can  be  encorporated  to  allow  the  user  to  speci¬ 
fy  the  search  area. 

V 


boundr les 

assign_boundry_points ( ) 

I 

boundr ies  boundry; 

boundry. xl  =  0; 
boundry. x2  -  48; 
boundry. yl  =  0; 
boundry. y2  =  34; 
boundry. zl  =  0; 
boundry.  z2  -=  15; 

return  (boundry); 

1 


/* 


FIND  SHORTEST  PATH 

- qQq - - - 

This  function  finds  the  shortest  path  and  assigns  the  global  pointer  to 
it*/ 


point 

f ind_shortest_path(start_path) 
paths  •start_path; 

{ 

/*  next  for  structure  is  used  only  to  find  shortest  path  */ 
paths  •current_path; 

point  pt; 

shortest_path  =  NULL; 

for  (current_path  =  start_path,  shortest_path  =  current_path; 
current_path  1=  NULL; 

currentpath  =  current_path->next_path)  { 

if  ( (current_path->total_cost  <  shortest_path->total_cost) 
&&  current_path->able2expand) 
shortest_path  =  current_path; 
if ( I shortest_path->able2expand ) 

shortest_path  =  shortest_path->next_path; 

) 

if  (plot28creen) 

id_shortest_path_for_expansion(shortest_path) ; 
return  (shortest_path->last_node->pt) ; 

) 
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APPENDIX  B 


#includ-  "3d  tan.h" 


/• 


BUILD  TWO  D  POLYGON  LIST 

- - - - - oOo - 

This  function  is  the  major  function  which  builds  the  two  dimensional  environment 
from  the  3  dimensional  environment.  It  accepts  as  a  parameterthe  perpendicular 
plane  (labeled  plane)  and  places  into  a  link  l^st  theintersection  points  of  a 
polyhedron  with  the  plane.  These  intersectionpoints  form  the  polygon. 

V 


polygon_llst  *build_two_d_polygon_list(start_2d,  plane,  null_ptr) 


( 


polygon_list 

equation 

int 


*start_2d; 
plane; 
null_ptr , 


obstacle_l 1st  ♦current_polyhedron  =  polyhedron_list ; 

polyhedron_obstacle  *current_face; 

poiyhedron_obstacle_plane  *current_node; 

polygon_llst  "polygon,  *last_polygon,  "previous; 

int  count,  intersection_count,  boolean  =  FALSE; 

static  int  polygon_number  =  1; 

point  ptl,  pt2; 


plane_number++ ; 
if (start_2d  l=  NULL) ( 

for ( last_polygon  =  start_2d;  /"finds  last  2d  polygon"/ 

last_polygon->next_polygon  1=  NULL; 

previous  =  last_polygon ,  last_polygon  =  last_polygon->next_polygon) ; 

) 

for  {current_polyhedron  =  polyhedron_list;  current_polyhedron  !=  NULL; 
current_polyhedron  =  current_polyhedron- >next_polygon )  [ 

if  (start_2d  ==  NULL)  ( 

NEW_P0LYG0N(start_2d) ; 
last_polygon  -  start_2d; 
plane_number  =  1; 

) 

else  { 

if  (null_ptr)  (  /"  create  a  new  node  if  the  last  node  is  not  null  "/ 
NEW_POLYGON ( 1 as t_polygon - >next_pol ygon )  ; 
previous  =  last_polygon; 

last_polygon  =  last_polygon->next_polygon; 

) 

1 

last_polygon->polygon_start  =  NULL; 
last_polygon->next_polygon  *  NULL; 
last_polygon->plane_number  =  plane_number ; 
last_polygon->polygon_number  =  polygon_number++ ; 

last_polygon->polyhedron_number  current_polyhedron->polyhedron  number; 

for  (current_face  =  current_polyhedron->obstacle,  intersectioncount  -  0; 
current_face  t=  NULL; 

current_face  =  current_face->next_face)  { 

for  (current_node  =  current_face->face_nodes,  count  =  1, 
boolean  *  FALSE; 

count  <=  current  face->face  nodes->number  nodes; 
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current_node  =  current_node->ccw,  count++)  { 
if  (boolean  ==  FALSE)  { 

ptl  =  intersectlon_point{current_node->pt,  current_node->ccw->pt, 

plane) ; 

if  (ptl.X  !=  NOINTERSECTION)  { 
boolean  =  TRUE; 
intersect lon_count++ ; 
break; 

) 

1 

] 

if  (boolean)  { 

last_polygon->polyqon_start  =  create_list(last_polygon->polygon_start, 

intersection_count,  ptl); 

1 

1 

if  (last_polygon->polygon_start  1=  NULL)  { 
null_ptr  -  TRUE; 

last_polygon->polygon_start->number_nodes  = 

count_polygon_nodes ( last_polygon )  ; 
last_polygon  =  reinove_duplicate_nodes(last_polygon) ; 
last_polygon  =  connect_links(last_polygon) ; 

1 

else 

null_ptr  =  FALSE; 


) 

if (last_polygon->polygon_start  ==  NULL) 
previous ->next_polygon  =  NULL; 
return  (start_2d); 


/* 


CREATE  LIST 

- oOo - 

This  function  accepts  the  polygon_start,  the 
parameters  and  places  the  point  (ptl)  into 
(polygon_start) . 

V 


intersection_count , 
the  link  list  for 


and  the  ptl 
2d  polygons 


obstacle_plane  *create_list(polygon_start,  intersection_count,  ptl) 
obstacle_plane  *polygon_start; 
int  Intersectioncount; 

point  ptl; 

obstacle_plane  *temp_ptr,  ‘current,  ‘previous; 
int  count; 

obstacle_plane  ‘two_d_ptr; 

if  (polygon_start  ”=  NULL)  ( 

NEW_PLANE(polygon_start) ; 

polygon_start->pt  •=  as8ign_point_values(ptl); 
polygon_start->number_nodes  =  intersection_count; 
poiygon_start->ccw  =  polygon_start->cw  ”  NULL; 

) 

else  { 

NEW_PLANE(temp_ptr) ; 

temp_ptr->pt  -  assign_point_values(ptl) ; 
temp_ptr->ccw  =  NULL; 

for  (previous  =  polygon_start,  current  =  polygon_start,  count  =  1; 
current  I-  NULL; 

previous  -  current,  current  =  current->ccw,  count++)  { 


if  (current->pt.x  ==  ptl.x  &&  current->pt .y  ==  ptl.y 
current->pt. z  “=  ptl.z)  { 
intersection  count  -=  1; 
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polygon_start- >number_nodes  =  intersection_count; 

free{temp_ptr) ; 

return  (polygon_start) ; 

) 

1 

previous ->ccw  =  temp_ptr; 

polygon_start->number_nodes  =  intersection_count; 

1 

return  (polygon_start) ; 


/* 


ASSIGN  POINT  VALUES 

- oOo - 

This  function  assigns  the  coordinate  values  (x,  y, 
a  point  structure 
*/ 


z) 


to  ptl  which  is  declared  as 


point 

point 

I 

point 


assign_point_values (pt2) 
pt2; 

ptl; 


ptl.x  =  pt2.x; 
ptl.y  =  pt2.y; 
ptl.z  =  pt2.2; 
return  (ptl); 


/* 


CONNECT  LINKS 

- oOo - 

This  function  connects  the  links  of  the  polygon_list  so  that  each  polygon  is  can 
be  accessed  using  a  doublely  linked  list 
V 


polygon_list  *connect_l inks (polygon) 

polygon_llst  ‘polygon; 

{ 

obstacle_plane  ‘current  =  polygon->polygon_start; 
int  count; 


for  (;  current->ccw  1=  polygon ->polygon_start;  current  =  current->ccw) 
current->ccw->cw  =  current; 


polygon->polygon_start->cw  =  current; 
return  (polygon); 


/• 


COUNT  POLYGON  NODES 

- qOo - 

This  function  counts  the  number  of  nodes  that  make  up  the  polygon  and  places  the 
value  in  the  the  polygon's  start  node  structure 
•/ 


int  count_polygon_nodes( polygon) 

polygon_list  ‘polygon; 

I 

obstacle_plane  ‘current  -  polygon->polygon_start->ccw; 
int  count  =  1; 

for  (;  current  I*  polygon->polygon_Btart; 
current  -  current->ccw,  count++) 
if  (current->ccw  — ■  NULL) 
current- >ccw  =  polygon->polygon_start; 
return  (count); 

1 
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/* 


REMOVE  DUPLICATE  NODES 


- Qq - 

This  function  removes  any  duplicate  nodes  which  are  placed  in  the  polygon  link 
listed  when  created 
V 


polygon_llst  *remove_duplicate_nodes{ polygon) 

polygon_list  *polvgon; 

{ 

obstacle_plane  *start  =  polygon->polygon_start, 

•current  -  start->ccw,  ‘previous  =  start; 

Int  countl,  number_duplicate_nodes  =  0; 

for  (countl  =  1; 

countl  <“  start- >number_nodes; 

countl++,  previous  -  current,  current  =  current->ccw)  { 

if  (current->pt.x  ==  previous->pt.x  && 
current- >pt.y  •==  previous->pt.y  && 
current->pt . z  =-  previous ->pt.z)  { 
number_dupl icate_nodes++ ; 

if  (start->pt.x  ==  current->pt . x  && 
start->pt.y  ==  current->pt.y 
start->pt.z  ==  current->pt.z)  { 
polygon->polygon_start  =  previous; 
previous ->number_nodes  -  current->number_nodes ; 

1 

prevlous->ccw  =  current->ccw; 
free((char  *)  current); 
current  =  previous->ccw; 

1 

1 

polygon->polygon_start->number_nodes  -=  number_duplicate_nodes ; 
return  ( polygon ) ; 


/* 


REMOVE  PLANES 

- - - qQq - 

This  function  deallocates  the  memory  associated  with  the  polygons  formed  by  the 
intersection  of  the  perpendicular  plane  and  the  polyhedron 
*/ 


polygon_l 1st  *remove_planes ( start_2d ) 
polygonlist  »start_2d; 

{ 

polygon_list  ‘previous  =  start_2d,  ‘current  =  start_2d->next_polygon; 
obstacle_plane  ‘nodel,  ‘node2; 

for(;  current- >next_polygon  I-  NULL; 

previous  =  current,  current  =  current->next_polygon ) ( 

for(nodel  "  previous->polygon_start,  node2  =  nodel->ccw; 
node2  I-  prevlous->polygon_start; 
nodel  -  node2,  node2  -=  node2->ccw) 
free( nodel ) ; 
free( nodel ) ; 

free ( previous ) ; 

] 

return{NULL) ; 
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APPENDIX  C 


^Include  "3d  tan.h" 


/* 


CREATE  OBSTACLE  LIST 


- OQq - 

This  function  is  the  main  function  used  to  create  the  3D  obstacle  environ-ment .  The 
list  of  obstacles  use  pointers  with  the  main  list  made  up  of  major  obstacles  only. 
Off  each  record  in  this  list  a  pointer  points  to  the  first  face  of  the  polygon, 
which  in  turn  points  at  the  next  one.  This  continues  until  a  list  of  faces  is 
established.  With  in  each  face  record  is  a  pointer  which  points  at  a  linked  list 
of  iiodes .  It  calls  create_face  passing  to  it  the  pointer  within  the  polygon  record 
that  points  to  the  first  face  of  the  polygon.  It  also  passes  the  file  pointer  to 
the  data  file*/ 


obstacle_list  *create_obstacle_lisc(void) 

1 

FILE  *fpt; 

int  count; 

obstacle_list  •polyhedron_llst,  ‘polygon  =  NULL; 

/»  opens  file  if  one,  else  exits  program  on  an  error  */ 
if  ((fpt  =  fopen( filename,  READONLY))  ==  NOLL)  { 

perror("  Error:  Data  file  did  not  open  correctly. \n  ");  exit(l); 

else  { 

read_comment ( fpt ) ; 

fscanf(fpt,  "%d",  4number_polygons) ; 

for  (count  =  1;  count  <•  number_polygons;  count++)  ( 

if  (count  “=  1)  (  ~ 

NEW_NODE(polyhedron_llst) ; 
polygon  =  polyhedron_list; 

} 

else  { 

NEW_NODE( polygon ->next_polygon) ; 
polygon  =  polygon- >next_polygon; 

) 

polygon->polyhedron_number  =  count; 

polygon->obstacle  =  create_face(polygon->obstacle,  fpt); 
polygon ->next_polygon  =  NULL; 

1 

1 

return  (polyhedron_list) ; 


/* 


CREATE  FACE 


- qQq - 

This  function  creates  the  list  off  the  major  polygon  list  and  makes  up  a  list  of 
polygon  faces.  It  call  create_plane,  passing  it  the  file  pointer  to  the  data  file 
and  the  pointer  within  each  face  that  will  point  to  the  first  node  of  the  linked 
node  list.  It  returns  a  the  start  record  (first  face  in  the  polygon  list)  to  the 
calling  function*/ 


polyhedron_obstacle  *create_face(start_face,  fpt) 
polyhedron_obstacle  *start_face; 

FILE  *fpt; 

( 

int  number_faces ,  count; 

polyhedron_obstacle  ‘face; 
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fscanf(fpt,  "%d",  fcnuinber_f aces )  ; 

for  (count  =  1;  count  <=  number_faces;  count++)  ( 
if  (count  “=  1)  ( 

NEW_POLYHEDRON ( start_f ace ) ; 
face  =  start_face; 

1 

else  [ 

NEWPOLYHEDRON ( face->next_face) ; 
face  *  face->next_face; 

) 

face->next_face  =  NULL; 

face->face_nodes  =  create_plane( fpt,  face->face_nodes ) ; 

1 

return  (startface) ; 


/* 


CREATE  PLANE 

- oOo - 

This  function  creates  the  floatly  linked  list  made  up  of  the  nodes  of  the  face  and 
returns  the  start  of  the  list  to  the  calling  function.*/ 


polyhedron_obstacle_plane  •create_plane( fpt,  start_plane) 
polyhedron_obstacle_plane  *start_plane; 

FILE  *fpt; 

I 

polyhedron_obstacle_plane  *current  =  start_plane,  *previous  =  NULL; 
int  count  =  1,  nodes; 

double  X,  y,  z; 

previous  ■=  NULL; 
fscanf(fpt,  *%d",  (nodes); 
for  (count  =  1;  count  <=  nodes;  count++)  ( 
fscanf(fpt,  "%lf  %lf  %lf",  &x,  &y,  &z); 
if  (count  ==  1)  { 

NEW_POLY_PLANE(start_plane); 
previous  =  start_plane; 
current  -  start_plane; 

) 

else  ( 

NEW_POLY_PLANE(current->ccw) ; 
current  =  current->ccw; 

) 

current->pt . X  =  x; 
current->pt . y  =  y; 
current->pt . z  =  z; 
current->cw  =  previous; 
current->ccw  =  NULL; 
previous  *  current; 

) 

start_plane->cw  »  current; 
current->ccw  =  startplane; 

8tart_plane->number_nodes  =  nodes; 
return  (start_plane) ; 


/* 


READ  COMMENT 


- oOo - 

This  function  is  used  only  to  strip  the  comments  off  the  polygon  data  file  so  that 
they  do  not  corrupt  the  link  list,  but  an  explanation  of  the  file  makeup  remains 
with  the  data  file 

*/ 


void  read_comment( fpt) 

FILE  'fpt; 

{ 
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char 


word [20] ; 


do 

fscanf(fpt,  "%s",  word); 
while  (word [2]  1= 


/* . . . - . - . 

DISTANCE 

- qOq - 

This  function  calculates  the  distance  from  one  point  to  another  and  re-turns  it  to 
the  calling  function*/ 


double 

dlstance(ptl,  pt2) 

point 

( 

double 

ptl,  pt2; 

a,  b,  c,  z; 

a  =  ptl.x  - 
a  -  sqr ( a ) ; 

pt2.x; 

b  -  ptl.y  - 
b  =  sqr(b); 

pt2.y; 

c  =  ptl.z  - 
c  =  sqr(c); 

pt2 . z; 

z  =  sqrt(a  +  b  +  c); 
return  (z); 

} 


/* 


FIND  PLANE  EQUATION 

.... - ... - qOq - - - - 

This  function  finds  a  plane  give  three  points.  It  finds  two  vectors  formed  by 
ptlpt2  and  ptlpt3,  then  calls  cross  product  to  find  the  a,  b  and  c  coefficents,  and 
finally  solves  for  d  using  a,b,c  and  ptl  »/ 


equation 

point 

{ 

point 

equation 


find_plane_equation(ptl,  pt2,  pt3) 
ptl,  pt2,  pt3; 

vectorl_2,  vector 13; 
pl_eq; 


vectorl_2 . x 
vectorl_2 . y 
vectorl_2 . z 
vectorl_3 . x 
vectorl_3 . y 
vectorl  3.Z 


pt2.x  -  ptl.x; 
pt2.y  -  ptl.y; 
pt2.z  -  ptl.z; 
pt3.x  -  ptl.x; 
pt3.y  -  ptl.y; 
pt3 . z  -  ptl.z; 


pl_eq  -  cross_product(vectorl_2,  vectorl_3); 

pl_eq.d  “  ((pl_eq.x  *  (-1.0)  *  ptl.x)  +  (pi_eq.y  *  (-1.0)  *  ptl.y)); 
pl_eq.d  =  pl_eq.d  +  (pi_eq.z  *  (-1.0)  »  ptl.z); 
return  (pl_eq); 


/* 


CROSS  PRODUCT 

- qQq - 

This  function  finds  the  cross  product  of  two  vectors  and  returns  the  coefficients 
to  the  calling  function.*/ 


equation 

point 

{ 

equation 


cross_product( vectorl ,  vector2) 
vectorl,  vector2; 

plane; 


plane. X  =  ( vector l.y  *  vector 2. z)  -  ( vectorl. z  *  vector2.y); 

plane. y  -  -1.0  *  ( (vectorl. x  *  vector2.z)  -  (vectorl. z  *  vector2.x)); 
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plane. z  =  (vectorl.x  *  vector2.y)  -  (vectorl.y  *  vector2.x); 
return  (plane) ; 


/* 


INTERSECT  POLYHEDRON 

- qOo - 

This  function  determines  if  the  line  formed  from  two  points  intersect  a  polyhedron. 
If  so,  it  returns  1,  else  it  returns  zero.  It  calls  f lnd_plane_equation,  passing 
it  three  nodes  on  the  current  face  being  checked  and  receives  the  plane  equation. 
It  then  calls  inter- sectlon_polnt  to  determine  if  the  line  intersects  the  plane. 
This  function  returns  the  point  if  there  is  an  intersection,  else  it  sets  the  x 
coeffi-cient  =  -999999.  If  there  is  an  intersection  point,  the  function  call 
lines_intersect,  which  determines  if  the  intersection  is  within  the  boun-dries  of 
the  plane. 

*/ 


int  lntersect_polyhedron(ptl,  pt2) 

point  ptl,  pt2; 

obstacle_list  *current_obstacle; 
polyhedron_obstacle  *face; 
polyhedron_obstacle_plane  ‘node; 
equation  pl_eq; 


for  (current_obstacle  =  polyhedron_list;  current_obstacle  1=  NULL; 
current_obstacle  =  current_obstacle->next_polygon)  ( 


for  (face  =  current_obstacle->obstacle;  face  1=  NULL; 
face  =  face->next_face)  { 


node  ■=  face->face_nodes; 

pl_eq  =  find_plane_equation(node->pt,  node->ccw->pt, 

node->ccw->ccw->pt) ; 


if  ((pl_eq.x 

(pl_eq.x 

continue; 


•  ptl.x  +  pl_eq.y  *  ptl.y  +  pl_eq.z  *  ptl.z  +  pl_eq.d)  0 

*  pt2.x  +  pl_eq.y  ‘  pt2.y  +  pl_eq.z  *  pt2.z  +  pl_eq.d)  ==  0) 

/*  if  ptl  lies  on  the  plane,  go  to  next  plane  •/ 


Intersection  =  intersection_point(ptl,  pt2,  pl_eq); 

if  ( intersection. X  !=  NOINTERSECTION)  { 

if  ( I lines_intersect( intersection,  face->face_nodes,  pl_eq)) 
return  (1); 

1 

1 

1 

return  (0); 


/♦ 


INTERSECTION  POINT 

- qQp - 

This  function  finds  the  intersection  of  a  plane  and  a  line.  It  accepts  three 
arguments,  2  points  (the  line)  and  an  equation  for  the  plane.  It  returns  the 
intersection  point  if  there  is  one  or  it  sets  the  plane. x  value  equal  to  -99999.0. 
This  value  is  a  flag  that  lets  the  calling  function  know  that  there  is  no 
intersection  point  and  the  line  and  plane  are  parallel.*/ 


point 

point 

equation 

( 

double 


inter8ection_point(ptl,  pt2,  plane) 
ptl,  pt2; 
plane; 

y  =  0.0,  t  =  0.0; 
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y  “  (plane. X  *  (pt2.x  -  ptl.x)  +  plane. y  *  (pt2.y  -  ptl.y)); 
y  »  y  +  (plane. z  *  (pt2.z  -  ptl.z)); 
if  (y  “  FALSE)  I 

intersection. X  =  NO_INTERSECTION; 
return  (intersection); 

) 

t  -  (plane. X  *  ptl.x  +  plane. y  *  ptl.y  +  plane. z  *  ptl.z  +  plane.d); 
t  -  -1  *  t  /  y; 

if  (0.0  <=  t  &&  t  <=  1.0)  ( 

intersection . X  =  ptl.x  +  (pt2.x  -  ptl.x)  *  t; 

Intersection. y  =  ptl.y  +  (pt2.y  -  ptl.y)  »  t; 

Intersection. z  -  ptl.z  +  (pt2.z  -  ptl.z)  *  t; 

) 

g1  S6 

intersection. X  -  NO_INTERSECTION; 
return  (intersection); 


/* 


LINES  INTERSECT  for  INSIDE  POLYGON  CHECK 

- qOo - 

This  function  determines  if  a  point  is  within  the  boundries  of  a  polygon.  If  the 
point  is,  the  function  returns  1,  else  it  returns  0.  Since  the  plane  can  be  on  any 
plane  in  the  3d  environment,  the  function  must  check  each  plane  face  (xy,  xz,  yz) . 

*/ 


int  llnes_intersect( intersect,  start,  pl_eq) 

point  intersect; 

polyhedron_obstacle_plane  *start ; 
equation  pl_eq; 

{ 

polyhedron_obstacle_plane  ‘current  =  start; 

int  number_nodes  =  start- >number_nodes,  count; 

int  intersect_count  =  0; 

double  t,  s,  b,  c; 

point  temp_pt; 

temp_pt  =  find_point_on_obstacle_plane_face(intersect,  pl_eq); 

for  (count  =  1;  count  <=  number_nodes;  count++,  current  =  current->ccw)  [ 

/*  CHECK  X  Y  PLANE  */ 

b  -  (temppt.x  -  intersect. X)  *  (current->ccw->pt . y  -  current->pt.y ) ; 
c  -  (temp_pt.y  -  intersect. y)  »  (current->ccw->pt . x  -  current->pt.x); 
b  =  b  -  c ; 

if  (b  1=  0)  { 

s  «  (intersect. y  -  current->pt.y)  *  (current->ccw->pt . x  -  current->pt .x) ; 
s  --  ( intersect. X  -  current->pt . x)  *  (current->ccw->pt.y  -  current->pt .y) ; 
8  •  s  /  b; 

t  =  (intersect. y  -  current->pt .y )  *  (temp_pt.x  -  intersect. x) ; 
t  •=  t  -  ( intersect. X  -  current->pt .x)  *  (temp_pt.y  -  intersect . y) ; 
t  =  t  /  b; 

if  (0  <=  s  s  <=  1  SS  0  <=  t  6S  t  <=  1)  ( 
intersect_count++ ; 
continue; 

) 

1 

/*  CHECK  X  Z  PLANE  */ 

b  -  (temp_pt.y  -  intersect. y)  •  (current->ccw->pt . z  -  current->pt . z ) ; 
c  -  {temp_pt.z  -  intersect. z)  *  (current->ccw->pt . y  -  current- >pt . y ) ; 
b  “  b  -  c; 
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if  (b  1=  0)  { 

s  “  (intersect. z  -  current->pt.z)  *  (current->ccw->pt.y  -  current->pt . y) ; 
8  (intersect. y  -  current->pt.y)  *  (current->ccw->pt . z  -  carrent->pt . z) ; 
s  *  8  /  b; 

t  “  (intersect. z  -  current->pt.z)  »  (tenip_pt.y  -  intersect . y) ; 
t  “  t  -  (intersect. y  -  current->pt.y)  *  (tenip_pt.z  -  intersect.z); 
t  “  t  /  b; 

if  (0  <=  8  &&  s  <=  1  ss  0  <=  t  t  <=  1)  ( 
intersect_count++ ; 
continue; 

) 

1 

/*  CHECK  Y  Z  PLANE  */ 

b  «  (temp_pt.x  -  intersect. X)  *  (current->ccw->pt. z  -  current->pt.z) ; 
c  -  (tenip_pt.z  -  intersect.z)  *  (current->ccw->pt.x  -  current->pt .x) ; 

if  (b  -  c  >=  le-3)  { 

s  -  (intersect.z  -  current->pt.z)  *  (current->ccw->pt.x  -  current->pt .x) ; 
s  (intersect. X  -  current->pt.x)  *  (current->ccw->pt . z  -  current->pt . z) ; 
8  -  8  /  (b  -  c); 

t  “  (intersect.z  -  current->pt.z)  *  (temp_pt.x  -  intersect .x) ; 
t  ”  t  -  (intersect. X  -  current->pt.x)  *  (temp_pt.z  -  intersect.z); 
t  -  t  /  (b  -  c); 

if  (0  <-  8  s  <-=  1  &&  0  <=  t  &S  t  <=  1)  { 
intersect_count++ ; 
continue; 

1 

1 

) 

if  ( inter8ect_count  %  2  ==  0) 
return  (1)7 
else 

return  (0); 


/* 


FIND  POINT  ON  OBSTACLE  PLANE  FACE 

- - -  - - oOo - - 

This  function  finds  a  point  that  is  on  the  plane.  It  is  called  by  the  function 
that  checks  if  the  intersection  of  the  ray  formed  by  two  points  intersects  any 
polyhedrons  in  the  environment.*/ 


point 

point 

equation 

I 

point 


f ind_point_on_obstacle_plane_face( intersect,  plane) 
intersect; 
plane; 

Pt; 


pt.z  “  -5; 

if  ((plane. X  »»  0  |  |  plane. y  ”=  0)  &6  plane. z  ■==  0)  ( 
pt.x  -  inter sect. x; 
pt.y  “  intersect. y; 

) 

else  ( 

pt.y  *  17; 
if  (plane. X  I-  0) 

pt.x  “  -(plane. d  +  plane. z  *  pt.z  +  plane. y  *  pt.y)  /  plane. x; 
else 

pt.x  «  50; 

) 

return  (pt); 


94 


/* 


VERTICAL  AND  PERPENDICULAR  PLANES 

- - - oOo - 

This  function  finds  the  vertical  and  perpendicular  planes 

V 


formed  by  two  points. 


void 

point 

{ 

point 


vert ical_n_per pend icular_plane{ptl,  pt2 ) 
ptl,  pt2; 

pt3; 


pt3.x  -  pt2.x; 
pt3.y  -  pt2.y; 
pt3.z  -  pt2.z  +  1; 

vertical  •  find_piane_equation(ptl,  pt2,  pt3); 
if  (ptl.x  I-  pt2.x)  ( 
pt3.x  -  pt2.x; 
pt3.y  -  pt2.y  +  1; 

) 

else  ( 

pt3.x  -  pt2.x  +  1; 
pt3.y  “  pt2.y; 

1 

pt3.z  -  pt2.z; 

perpendicular  -  find_plane_equation(ptl,  pt2,  pt3); 
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APPENDIX  D 


Klnclude  "3d  tan.h" 


/• 


CHECK  ACTIVE  NODE  LIST 
- OOO - - 


This  function  checks  the  active  node  list  to  determine  if  the  expanded 
node  found  in  f ind_tangents_froro_inner_nodes  has  already  been  used  as  a 
node  to  an  existing  path.  If  the  point  has  been  used,  the  function  deter¬ 
mines  if  the  possible  new  path  has  a  cost  less  than  the  existing  path.  If 
the  cost  is  less,  the  function  calls  the  function  mark_old_path_with_node_ 
unexpandable,  which  marks  the  path  as  unexpandable.  If  the  new  possible 
path  has  a  smaller  cost,  then  the  function  returns  a  one  (1)  else  it 
returns  a  zero  (0). 

"/ 


int 

active_nodes 

paths 

point 

( 

active_nodes 

a_path 


check_active_node_list (head_active_nodes , 
•head_active_nodes ; 

•shortest ; 

pt; 

•current  •=  head_actlve_nodes; 

•  last  node  *=  shortest->last  node; 


shortest , 


pt) 


for  ( ;  current  I-  NULL;  current  •=  current->next_active_node)  ( 

if  (current->pt  .x  —  pt.x  &&  current->pt . y  --  pty  &&  current ->pt .  z  “=  pt.z) 

{ 

if  (current->distance  >  (shortest->cost  + 

dlstance(shortest->last_node->pt,  pt)))  ( 

8tart_path  « 

ma rk_old_path_with_node_unexpandable( cur ren t ->when_path_ found ) ; 
return  (1); 

1 

else 

return (0 ) ; 

1 

I 

return(  1)  ; 

1 


/* 


MARK  OLD  PATH  WITH  NODE  UNEXPANDABLE 
. OOO . 


This  functions  is  called  if  the  existing  path  has  been  determined  as  too 
expensive  (le;  there  is  a  node  in  the  path  that  can  be  reached  by  another 
path  with  a  smaller  cost).  The  function  searches  the  path  list  to  find 
the  correct  path  and  then  marks  the  flag  "able2expand"  as  FALSE  (0). 

*/ 


paths 

int 

( 

paths 


•mark_old_path_with_node_unexpandable(path_number ) 
path_nu!nber  ; 

•current  -  start_path; 


for  (;  current  I-  NULL;  current  -  current->next_path)  ( 
if  (current->when_path_found  path_number) 
current->able2expand  ”  FALSE; 

) 

return  (start_path) ; 
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/* . 

INSERT  NODES  INTO  ACTIVE  NODE  LIST 

. - . - . OOO . . */ 

active_nodes  •insert_nodes_into_active_node_list ( head_node_lis t ,  pt , 

cost) 

activenodes  *head_node_list; 
point  pt; 

double  cost; 

{ 

static  active_nodes  *last_node_added  =  NULL; 

if  ( head_node_l ist  ==  NULL)  ( 

NEWACTI VE_NODE ( headnodel ist ) ; 
last_node_added  =  head_node_list; 
last_node_added->next_active_node  =  NULL; 

1 

else  ( 

NEW_ACTIVE_NODE ( last_node_added- >next_active_node) ; 
last_node_added  •  last_node_added->next_active_node; 
last_node_added- >next_active_node  =  NULL; 

1 

last_node_added->pt  =  ass ign_point_values (pt ) ; 
last_node_added->distance  =  cost; 

last_node_added->when_path_found  =  number_pa t)is_ found ; 
return  ( head_node_l ist ) ; 

) 
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APPENDIX  E 


This  appendix  contains  the  source  code  I  began  to  implement  for  going  up,  around 
and  up  type  of  paths  as  described  in  Chapter  IV.  The  file  over&@.c  contains  the 
source  code  developed  todate  and  not  yet  Implemented 

#include  "3d_tan.h" 
jtinclude  "plot.h" 

idefine  OVER  0 

paths  *up_around_up(path,  tan,  start,  goal) 
paths  ‘path; 
int  tan; 

[/* 

polyher'  on_face  *from_face  =  NULL,  *to_face  =  NULL; 
obstacle_liot  *polyhedronl,  *polyhedron2; 
a_path  *tangent_llst; 
vertlcal_path  •vertical_list; 
int  count; 

polygonal ist  ‘polygons,  *recursive_polygons  =  NULL; 
paths  *partial_path  =  NULL,  ‘shortest,  ‘short_list; 

for (polyhedronl  =  polyhedron_llst; 
polyhedronl  !=  NULL; 

polyhedronl  -  polyhedronl- >next_polygon ) { 
from_face  =  find_polyhedron_face(polyhedronl) ; 

for (polyhedron2  =  polyhedron_list; 
polyhedron2  1=  NULL; 

polyhedron2  =  polyhedron2->next_pol/gon) { 

if (polyhedron2->polyhedron_number  ==  polyhedronl ->'polyhedron_number  | i 
polyhedron2->polyhedron_number  ==  path->polygon4vert) 
continue; 
else 

to_face  =  f ind_polyhedron_face(polyhedron2 ) ; 

/‘since  tangent  returns  a  tangent  between  two  polygons,  have  to  form  a 
polygon  list  with  only  the  to_face  and  from  face  in  the  list  so  that  a 
the  tangent  is  found‘/ 

polygons  =  make_list{ from_face,  toface); 
for(count  =  1;  count  <=  4;  count++){ 

lf(NULL  ==  (tangent_list  =  tangent(path,  from_face,  to_face->face_nodes, 
(count  ==1 1  I 37MINUS: PLUS) ,  polygons,  OVER, 
count  ==l| |2?MINUS:PLUS)))  printf("you  blew  it  caddell"); 

if ( lchech_tangent_list(tangent_list,  path))  continue; 

if ( I lntersect_polyhedron( tangent_list->pt,  tangent_list->next_node->pt) ) 
continue; 
else( 

vertical_list  -  f ind_vertical_nodes ( tangent_list->next_node- >pt , 

tangent_llst->pt) ; 

vertical_n_perpendicular_plane(vertical_list->next_node->next_node->pt , 
path->la8t_node->pt); 

recurs ive_polygons  *  build_two_d_polygon_list (recursive_polygons , 

perpendicular,  FALSE); 

partlal_paths  =  dupllcate_path{path) ; 
partial_paths->plane_number  “  plane_number ; 
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partial_paths  =  continue_vertical_path(partial_paths,  recurs ive_polygons 


recurs ive_polygons  =  free_polygon_list(recursive_polygons) ; 

1 

)/*end  count  for  structure*/ 

)  /*end  inner  for  structure*/ 

)  /*end  outer  for  structure*/ 

f ree_pseudo_polygon_list ( polygons ) ; 
f ree_tangent_list ( tangent_list ) ; 

*/ 

return (path) ; 


polyhedron_face  *f ind_polyhedron_f ace (polyhedron) 
obstacle_list  *polyhedron; 

{ 

polyhedron_face  *face,  *inln_vert_face  =  NULL; 
obstacle_plane  *node,  *inin_vert_node  =  NULL; 

for(face  =  polyhedron->obstacle; 
face  1=  NULL; 
face  “  face->next_face) ( 

node  =  face->face_nodes; 

if ( node->pt . z  1=  node->ccw->pt.z  && 

node->pt.z  !=  node->cw->pt.z)  continue; 
else( 

if (NULL  ==  niin_vert_face)  min_vert_face  =  face; 
else{ 

if (min_vert_face->face_nodes->pt.z  <  node->pt.z)  break; 
else  min_vert_face  =  face; 

1 

1 

return(itiin_vert_face)  ; 

1 


polygon_llst  *make_llst(to,  from) 
obstacle_plane  *to,  *from; 

{ 

polygonlist  *polygons; 

NEW_POLYGON ( polygons ) ; 

NEW_POLyGON (polygons->  next_polygon ) ; 

polygons->polygon_start  =  from; 

polygons ->next_polygon->polygon_start  =  to; 

polygon8->next_polygon->next_polygon  -  NULL; 

return  ( p>olygons ) ; 

1 

int  free_pseudo_polygon_list (polygons ) 
polygon_list  *polygons; 


I 


free(polygon8->next_polygon) ; 
free(polygon8 )  ; 
return(l) ; 
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int  chech_tangent_list(tangent_list,  path) 
a_path  *tangent_list; 
paths  ‘path; 


double  dir; 

if (path->forward)  dir  -  goal_direction; 
else  dir  =  norm(goal_direction  -  HPI); 

if (ABS{noriii(dir  - 

atan2(tangent_list->pt.y  -  path->last_node->pt . y, 
tangent_list->pt . X  -  path->last_node->pt.x) ) )  >=HPI) 
return(O) ; 

if (abs(norin(dir  - 

atan2(tangent_list->next_node->pt.y  -  path->last_node->pt . y, 
tangent_list->next_node->pt . X  -  path->last_node->pt . x) ) )  >=  HPI) 
return(O) ; 

return(l) ; 

1 

a_path  *free_tangent_list(llst) 
a_path  ‘list; 

( 

a_path  ‘previous,  ‘current; 

for(current  =  list,  previous  =  list; 
current  1=  NULL; 

previous  •=  current,  current  =  current->next_node,  free(previous) ) 
free( previous) ; 
return (NULL) ; 


polygon_list  ‘free_polygon_list(list) 
polygon_list  ‘list; 

{ 

polygon_list  ‘previous_poly ,  ‘current_poly; 
obstacle_plane  ‘node; 
int  node_nuinber ,  count; 

for (current_poly  =  list,  previous_poly  =  list; 

NULL  I-  current_poly ; 

previous_poly  =  current_poly ,  current_poly  =  current_poly- >next_pOiygon , 
free(previous_poly) ) { 

for (node  =  current_poly->polygon_start, 

node_number  =  current_poly->polygon_start->number_nodes, 
count  =  1; 

count  <=  node_nuinber ; 

node  ”  node->ccw,  node->cw->ccw  =  NULL,  f ree( node->cw) ,  count++); 

) 

f ree(previou8_poly ) ; 
return(NULL); 
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APPENDIX  F 


This  appendix  contains  the  source  code  found  in  path.c 

# include  "3d_tan.h" 

♦include  "plot.h" 

♦define  LOOPFOREVER  1 
♦define  AROUND  PATH  1 


/* 


REMOVE  PATHS 

- oOo - 

This  function  frees  from  memory  the  created  possible  paths  so  that  another 
iteration  of  path  building  can  commence.  It  accepts  the  head  of  the  list 
and  returns  a  NULL  pointer  to  the  head  of  the  list.*/ 


paths 

paths 

{ 

paths 

a_path 

int 


*remove_paths (start,  shortest) 

‘start,  ‘shortest; 

‘current  =  start->next_path,  ‘previous  =  start; 
‘node,  ‘nodel; 
i; 


for  (previous  =  start,  current  =  start->next_path; 
current ->next_path  !=  NULL; 

previous  =  current,  current  =  current->next_path)  ( 

if  (shottest_path  ==  previous)  { 
shortest->next_path  =  NULL; 
continue; 

) 

for  (node  =  previous->last_node,  nodel  =  node->next_node; 
nodel  !=  NULL; 

node  =  nodel,  nodel  =  nodel->next__node) 
free (node);  ~ 

free  (node);  /‘  frees  last  node  in  the  path  being  deleted  ‘/ 

free (previous) ; 

) 

if  (shortest_path  !=  current)  { 
previous  =  current; 

for  (node  =  previous->last_node,  nodel  =  node->next_node; 
nodel  !=  NULL; 

node  =  nodel,  nodel  =  nodel->next_node) 
free (node) ; 

free (node);  /‘  frees  last  node  in  the  path  being  deleted  ‘/ 

free (previous) ;  /‘  frees  last  path  in  the  path  list  ‘/ 

} 

start  =  shortest; 
return  (start) ; 
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/* - 

ORDER 

- oOo - 

This  function  uses  the  area  function  for  a  triangle  in  determining  if  the 
current  node  is  the  common  tangent.  It  uses  one  of  the  two  adjacent  nodes 
and  the  inputted  point  to  form  the  triangle.  It  returns  the  value  found 
(double)  in  the  calculations.*/ 


double  order (pi,  current,  ad jacent_node) 

point  pi; 

obstacle_plane  ‘current,  *ad jacent_node; 

{ 

double  z; 


z  =  (current->pt .X  -  pl.x)  •  (ad jacent_node->pt . y  -  pl.y); 
z  =  z  -  (ad jacent_node->pt .X  -  pl.x)  •  (current->pt . y  -  pl.y); 
return  (z) ; 


/* 


START  FINDING  PATHS 

- oOo - 

This  function  finds  all  the  valid  tangential  lines  from  the  start  point 
to  polygons  in  the  start_2d  list  and  places  them  in  the  start_paths  link 
list.  It  returns  this  link  list  to  the  calling  function.  The  function 
calls  plus  and  minus  tangent  functions  to  find  the  tangents  from  the  start 
to  all  the  polygons  in  the  start_2d  list.  As  it  finds  the  two  tangents 
for  a  polygon,  it  calls  cross_polygon  to  determine  if  there  is  not  a  poly¬ 
gon  between  the  point  and  the  start  point.  If  not,  then  the  node  is  added 
to  the  list  as  the  beginning  of  a  valid  path,  by  calling  create_path_n_nodes . 
It  calls  asslgn_polnt_values,  which  places  the  new  point  into  the  link  list. 
The  function  also  calls  the  function  atan2,  which  is  a  library  function  that 
determines  the  direction  from  the  start  point  to  the  node  added  to  the 
start_paths  list.*/ 


paths 

paths 

polygon_list 
point  ~ 
boundr ies 
active  nodes 


start_f indingjpaths (start_paths,  start_twod, 
boundry,  active_node_list) 
*start_paths; 

*start_twod; 
start_pt,  goal; 
boundry; 

‘active  node  list; 


start_pt,  goal. 


{ 


paths 

polygon_list 

obstacle_plane 

int 

point 


*current_path  =  NULL; 
*current_polygon; 
*current_node; 
count,  tangent; 
ptl,  pt2; 


plane_number  = 


1;  /*  tells  what  plane  the  2d  representation  is 

*  on  */ 


vertical_n_perpendicular_plane (start_point,  goal)  ; 

/*start_2d  is  a  global  defining  the  two  d  polygon  list*/ 

start_2d  =  build_two_d j)olygon_list {start_2d,  perpendicular,  FALSE); 

#ifdef  DOS 

if  (plot2screen) 

data_plot (start_2d) ; 

#endif 


for  (current_polygon  =  start_2d; 
current_polygon  !=  NULL; 

current_polygon  =  current_polygon->next_polygon)  ( 

ptl  =  plus_tangent (current_polygon->polygon_start,  start_pt); 
pt2  =  minus_tangent (current_polygon->polygon_start,  start_pt) ; 
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if  (dlrection_heuristic_from_start <start_pt ,  ptl)  && 
boundry_check (boundry,  ptl))  ( 
if ( !cross_polygon (start_2d,  start_pt,  ptl)){ 

start_paths  =  create _path_n_nodes (start_paths,  current_path) ; 
tangent  =  PLUS; 
if  (current_path  ==  NULL) 

current_patli  =  start_pat)is; 
else 

current_path  =  current_path->next_patli; 
currentjpath  =  update_current_path (current_path,  /*start_pat)is, */ 

tangent,  start_pt,  ptl,  goal, 
current_polygon->polygon_number ) ; 
current_path  =  add_start_point_to_path (current_patli,  start_pt); 


#ifdef  DOS 
if 


#endif 


(plot2screen) 

draw_line_segment (start_pt, 
REGULAR) ; 


current_patli->last_node->pt,  YELLOW, 


else  ( 

if  ( lliorizontal)  ( 

start_pat)is  =  up_and_over  (start_patlis,  current_path,  start_point, 

ptl,  start_point,  PLUS, 
current_polygon->polyhedron_number) ; 
if (current_path  ==  NULL)  current_path  =  start  paths; 
else  current_path  =  current_path->next_path;  ~ 
current_path->f orward  =  YES; 

) 

} 

) 

if  (direction_heuristic_f rom_start (start_pt,  pt2)  iS 
boundry_chec)t  (boundry,  pt2))  { 
if  ( !cross__polygon  (start_2d,  start_pt,  pt2))( 

start_paths  =  create_path_n_nodes (start_paths,  current  path); 
tangent  =  MINUS; 
if  (current_path  ==  NULL) 

current_path  =  start_paths; 
else 

current_path  =  current_path->next  jpath; 
current_j>ath  =  update_current_path (current_path,  tangent,  start_pt, 

pt2,  goal, 

current_polygon->polygon_number) ; 
current_path  =  add_start_point_to_path (current_path,  start_pt); 


#ifdef  DOS 
if 


#endif 


(plot2screen) 

draw_line_segment (start_pt,  current_path->last_node->pt, 

REGULAR) ; 


YELLOW, 


) 


) 

else  { 

if ( Ihorizontal) { 

start_paths  =  up_and_over (start_paths,  current_path,  start_point, 

pt2,  start_point,  MINUS, 
current_polygon->polyhedron_number ) ; 
if (current_path  ==  NULL)  current_path  =  start_paths; 
else  current_path  =  current_path->next_path; 
current_path->forward  =  YES; 


) 


/*this  function  finds  the  one  partial  vertical  path  from  the  start 
point  over  all  obstacles  to  goal*/ 

start_paths  =  up_and_over <start_paths,  current_path,  start_point,  goal, 
start_point,  VERTICAL) ; 
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current_path->next_path->forward  =  YES;  /•current_path->next  is  path 

added  to  the  list  that  is 
up  and  over  all  obstacles*/ 
current_path  =  current_path->next_path; 

/*this  function  finds  all  the  partial  vertical  paths  from  the  goal,  thus 
working  backwards  to  find  all  the  paths  which  go  around  one  or  more 
obstacles  and  over  at  least  one  obstacles*/ 
start_paths =  up_and_over_from_goal (start_paths,  current_path,  goal,  start_2d) 

return  (start_paths) ; 

} 


/* 


ADD  START  POINT  TO  PATH 

- oOo - 

This  function  adds  the  start  point  to  the  partial  list  being  created  by 
the  function  start  finding  paths.*/ 


paths 

paths 

point 

{ 

a_path 


*add_start_point_to_path (current_path,  start_point ) 
*current_path; 
start_point; 

♦current  node; 


for  (current_node  =  current_path->last_node; 
current_node->next_node  !=  NULL; 
current  node  =  current  node->next  node) ; 


NEW_NODES (current_node->next_node) ; 

curtent_node->next_node->pt  =  assign_point_values (start_point) ; 
current_node->next_node->next_node  =  NULL; 
current_node->next_node->next_after_node  =  current_node; 
return  (current_path) ; 


/* 


CREATE  PATH  AND  NODES 

- oOo - 

This  function  is  called  to  create  and  insert  into  the  startjpaths  link 
list  new  points  which  form  valid  paths  from  the  start,*/ 


paths  *create_path_n_nodes (start_paths,  current_path) 

paths  *start_paths,  *current  path; 

{ 

if  (start_paths  ==  NULL)  { 

NEW_PATH (start_paths) ; 
current_path  =  start  paths; 

) 

else  { 

NEW_PATH (current_path->next_path) ; 
current_path  =  current_path->next_path; 

) 

current_path->polygon4vert_PTR  =  NULL; 
current_path->next  jpath  =  NULL; 

NEW_NODES (current_path->last_node) ; 
current_path->last_node->next_node  =  NOLL; 
return  (start_paths) ; 


) 
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/* 


CROSS  POLYGON 
- oOo - 


This  function  determines  if  a  line  crosses  the  polygon.  It  checks  each 
line  segment  of  the  polygon  and  returns  1  if  there  is  an  intersecttlon . 
The  second  for  structure  is  used  to  check  if  p2  is  on  a  polygon.  If  so, 
boolean  is  asserted  to  true  (int  value  =  1) .  This  tells  the  rest  of  the 
for  loop  to  go  on  to  the  next  polygon*/ 


int  cross_polygon (start,  pi,  p2) 

polygon_list  *start; 
point  pi,  p2; 

{ 

int  count,  boolean; 

polygon_list  ‘polygon  =  NULL; 

obstacle_plane  ‘current  =  NULL; 
point  p3,  p4; 

double  w,  X,  y,  z; 


for  (polygon  =  start;  polygon  !=  NULL; 

polygon  =  polygon->next_polygon)  { 


for  (count  =  1,  current  =  polygon->polygon_start,  boolean  =  FALSE; 
count  <=  polygon->polygon_start->number_nodes; 
count++,  current  =  current->ccw)  { 
if  (p2.x  ==  current->pt . X  &&  p2.y  ==  current->pt .y)  { 

boolean  =  TRUE; 
break; 

) 

) 


if  ((boolean)  { 

for  (count  =  1,  current  =  polygon->polygon_start; 

count  <=  polygon->polygon_start->number_nodes; 
count++,  current  =  current->ccw)  { 
p3.x  =  current->pt . X,  p3.y  =  current->pt .y; 
p4.x  =  current->ccw->pt .X,  p4.y  =  current ->ccw->pt.y; 
w  =  sign ( function_one (p3 . X,  p3.y,  pi,  p2)); 

X  =  sign ( functlon_one (p4 . X,  p4.y,  pi,  p2)); 
y  =  sign (function_two (pi . X,  pl.y,  p3,  p4)); 
z  =  sign ( functlon_two (p2 . X,  p2.y,  p3,  p4)); 

if  (w  !=  X  y  !=  z)  { 

return  (1);  /‘  line  intersects  polygon  ‘/ 

) 

) 

current  =  current->ccw; 

) 

> 

return  (0 ) ; 


/‘ 


FUNCTION  ONE  AND  FUNCTION  TWO 
- oOo - 


Funcitons  one  and  two  are  simple  line  equaitons  used  to  determine  if  two 
lines  intersect.  They  both  return  the  value  of  the  equation  to  the 
calling  funcion*/ 


double  function_one (x,  y,  pi,  p2) 

double  X,  y; 

point  pi,  p2; 

{ 

int  z  =  0; 


z  =  ((y  -  pl.y)  ‘  (p2.x  -  pl.x))  -  ((p2.y  -  pl.y)  *  (x  -  pl.x)); 
return  ( z) ; 
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> 


double 

double 

point 

int 


function_two (x,  y,  p3,  p4) 
X,  y; 
p3,  p4; 

z  =  0; 


z  =  {(y  -  p3.y)  *  (p4.x  -  p3.x))  -  ((p4.y  -  p3.y)  *  (x  -  p3.x)); 
return  (z)  ; 


/* 


DUPLICATE  PATH 

- oOo - 

This  function  accepts  from  the  calling  function  a  valid  path,  duplicates 
it  and  inserts  the  duplication  into  the  start_paths  link  list.  It  calls 
duplicate_entries  to  duplicate  the  header  data  of  each  path  (ie  type  of 
tangent,  total  cost,  estimated  cost,  direction  etc...)*/ 


paths 

paths 

{ 

a_path 

paths 


*duplicate_paths (current) 
‘current; 

‘node,  ‘duplicate_node; 
‘duplicate; 


duplicate  =  duplicate_entries (current) ; 
for  (node  =  current->last_node; 

.ncde  ;=  NULL; 
node  =  node->next_node)  ( 
if  (duplicate->last_node  ==  NULL)  { 

NEW_N0DES (duplicate->last_node) ; 
duplicate_node  =  duplicate->last_node; 
duplicate_node->next_after_node  =  NULL; 

) 

else  ( 

NEW  NODES (duplicate_node->next_node) ; 

dupTicate_node->next_node->next_after_node  =  duplicate_node; 
duplicate  node  =  duplicate_node->next  node; 

} 

duplicate_node->next_node  =  NULL; 

duplicate_node->polygon_number  =  node->polygon_number ; 
duplicate_node->pt  =  assign_point_values (node->pt) ; 

) 

return  (duplicate) ; 


/‘ 


DUPLICATE  ENTRIES 

- oOo - 

This  function  duplicates  the  major  entries  of  a  valid  path  needed  in  the 
calculations  for  finding  the  best  path*/ 


paths 

paths 

{ 

paths 


‘dupllcate_entrles  (current) 
‘current; 

‘duplicate; 


NEW  PATH (duplicate)  ; 
dupTicate->last_node  =  NULL; 
duplicate->cost  =  current->cost; 

dupllcate->estimated_cost  =  current->estimated_cost; 
duplicate->total  cost  =  current->total_cost; 
duplicate->ray_dTrection  =  current->ray_direction; 
duplicate->plane_number  =  current->plane_number; 
duplicate->type_tangent  =  current->type_tangent; 
duplicate->forward  =  current->forward; 
duplicate->polygon4vert_PTR  =  NULL; 
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return  (duplicate) ; 


EXTEND  PATH 

- oOo - 

This  function  accepts  three  parameters  (the  start  path  for  either  possible 
verticle  paths  of  possible  perpendicular  paths,  the  start  pointer  for  the 
2d  polygon  representation,  and  the  goal  point.  It  searches  the  possible 
path  list  for  the  shortest  total  distance  from  start  to  goal.  It  checks 
the  line  projection  of  this  path  and  the  goal  with  the  goal  point  for  any 
intersection  with  polygons  from  the  polygon  list  (start_2d  list).  If  no 
intersections,  the  goal  is  appended  to  this  shortest  path  and  the  short¬ 
est  path  is  returned  to  the  the  calling  funcition.  If  there  is  an  inter¬ 
section,  the  function  calls  FIND_TANGENTS  FROM_INNER_NODES,  passing  to  it 
the  shortest  path  and  the  start_2d  pointer 


paths 


polygon_list 
point 
paths 
boundr ies 
active  nodes 


*extend_path (start,  goal,  shortest_path,  start_2d,  boundry, 
active_node_list) 

*start_2d; 

start,  goal; 

*shortest_p3th; 

boundry; 

‘active  node  list; 


paths 

a_path 


*current_path  =  start_path; 
‘new  record,  ‘current; 


if  (shortest_path->type_tangent  !=  NONE)  { 

shortest_path  =  f ind_tangents_f rom_inner_node (shortest_path,  start_2d, 

boundry,  active_node_list , 

1)  ; 

shortest_path->able2expand  =  NO; 

} 

else  { 

if (shortest_path->forward  ==  YES)  /‘partial  path  is  from  start  or 

forward‘/ 

start_2d  =  add_on_new_plane_intersection (start_2d, 
shortest_path->last_node, 
goal) ; 

else  /‘partial  path  is  from  the  goal  or  backwards*/ 
start_2d  =  add_on_new_plane_intersection (start_2d, 
shortest_path->last_node, 
start) ; 

shortest_path->plane_number  =  plane_number; 

shortest_path  =  up_around_up(shortest_path,  PLUS,  start_point,  goal); 
shortest_path  =  continue_vertical_path (shortest_path,  start_2d) ; 

) 

return  (shortest_path) ; 


GOAL  REACHED  UPDATE 

- oOo - 

This  function  appends  the  goal  point  to  the  shortest  path  if  the  last  node 
of  the  shortest  path  list  and  the  goal  point  are  visible  to  each  other, 
it  then  returns  the  entire  list  of  possible  paths  back  to  the  calling 
function . */ 


paths  *goal_reached_update (goal,  shortest,  partialjpath) 

point  goal; 

paths  ‘shortest; 

a_path  ‘partial_path; 
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a_path 

double 


new_record,  *temp; 
cost  =  0; 


#ifdef  DOS 

if  ( !plot2screen) 

printf ( "\nYou  have  reached  the  goalVn"); 

#ellf 

printf ( "\nYou  have  reached  the  goal\n")  ; 


#endif 

for (temp  =  partial_path;  temp->next_node  !=  NULL; 
temp  =  temp->next_node) 

cost  +=  distance (temp->pt,  temp->next_node->pt)  ; 

temp->next_node  =  shortest->last_node; 
shortest->last_node  =  partial_path; 
cost  +=  distance (temp->pt,  temp->next_node->pt ) ; 
shortest->cost  +=  cost; 

NEW_N0DES (new_record) ; 

new_record->pt  =  assign_point_values (goal) ; 

shortest->cost  +=  distance (shortest->last_node->pt,  new_record->pt ) ; 

new_record->next_node  =  shortest->last_node; 

shortest->last_node  =  new_record; 

shortest->total_cost  =  shortest->cost; 

shortest->estlmated_cost  =  0; 

shortest->type_tangent  =  NONE; 

return  (shortest); 

) 


/* 


FIND_TANGENTS_FROM_INNER_NODES 

- oOo - 

This  function  finds  all  valid  tangents  from  an  inner  node  to  other  poly¬ 
gons.  It  is  very  similiar  to  the  start_f inding_paths  function;  however 
it  checlcs  the  direction  of  the  possible  new  node  of  the  path  with  the 
current  direction  of  the  path  to  determine  if  the  node  is  valid.  It  also 
calls  a  variation  of  cross_polygon  (cross_polygon_node_on)  that  takes 
into  consideration  that  both  the  node  that  form  a  line  lie  on  a  polygon.*/ 


paths  *find_tan'.  «;nts_from_inner_node  (shortest,  start_2d,  boundry, 

head_node  list,  recursion  number) 


paths 

polygon_list 
boundries 
active  nodes 
int 

{ 

polygon_list 

paths 

a_path 

‘shortest ; 

*start_2d; 

boundry; 

* head_node_l i st ; 
recursion  number; 

‘polygon,  ‘previous,  ‘polygonl; 

‘expanded,  ‘same_polygon  =  NULL; 

‘new_record; 

#ifdef  DOS 
a  path 
#endTf 

‘print_path; 

point 

a_path 

ptl,  pt2; 

‘current  =  shortest->last  node, 

‘minus_tangent_list, 

‘plus_tangent_llst; 

for  (polygonl 

=  start_2d;/*  finds  the  polygon  the  last  node  of  the 
*  shortest  path  lies  on  */ 

polygonl->polygon_number  !=  shortest->last_node->polygon_number  I  I 
polygon l->plane_number  !=  shortest->plane_number; 
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polygonl  =  polygonl->next_polygon) ; 

for  (polygon  =  start_2d; 
polygon  l~  NULL; 

polygon  =  polygon->next_polygon)  { 
on_same_polygon  =  FALSE; 

if (shortest->plane_number  <  polygon->plane_number ) 

break;  /‘planes  found  after  the  plane  which  shortest  path  last  node 
is  are  not  checked*/ 

if  (shortest->plane_number  !=  polygon->piane_number  | | 
shortest->last_node->polygon_number  ==  polygon->polygon_number ) 
continue; 


minus_tangent_list  =  tangent (shortest,  polygonl,  polygon->polygon_start , 
MINUS,  start_2d,  AROUND_PATH) ; 

plus_tangent_list  =  tangent (shortest,  polygonl,  polygon->polygon_start, 
PLUS,  start_2d,  AROUND_PATH> ; 


#ifdef 


if  (dlrection_heurlstlc4tangent (shortest,  plus_tangent_list) 
boundry_check (boundry ,  plus_tangent_list->pt)  && 
boundry_check (boundry,  plus_tangent_list->next_node->pt ) ) 
if (NULL  !=  plus_tangent_list  && 

check  active  node  list (head  active  nodes,  shortest. 


&& 

{ 


if  (plot2screen) { 

for (print_path  =  plus_tangent_list;  print_path->next_node  != 
print_path  =  print_^ath->next_node) 
draw_line_segment (print_path->pt,  print_path->next_node->pt 


NULL; 

YELLOW, 


#endif 


REGULAR) ; 

draw_line_segment (print_path->pt,  shortest->last_node->pt ,  YELLOW, 
REGULAR); 


paths_found++ ; 

expanded  =  duplicate_paths (shortest) ; 

shortest  =  expand_paths_with_values (shortest,  expanded, 

pius_tangent_list,  head_node_list , 
polygon->poiygon_number,  PLUS)  ; 

if  (on_same_polygon) 
same_polygon  =  expanded; 

) 

) 


#ifdef 


if  (direction_heuristic4tangent (shortest,  ininus_tangent_list)  && 
boundry_check (boundry,  minus_tangent_list->pt )  && 
boundry_check (boundry,  minus_tangent_list->next_node->pt) )  ( 

if (NULL  !=  minus_tangent_list  && 

check_active_node_iist (head_active_nodes,  shortest, 
minus_tangent_list->pt) )  ( 

DOS 

if  (plot2screen) ( 

for (print_path  =  minus_tangent_list;  pririt_path->next_node  !=  NULL; 
print_path  =  print_path->next_node) 
draw_line_segment (print_path->pt,  print_path->next_node->pt,  YELLOW, 
REGULAR) ; 


draw_line_segment (prlnt_path->pt,  shortest->last_node->pt,  YELLOW, 

REGULAR) ; 


) 


#endif 


paths_found++; 

expanded  =  duplicate_paths (shortest); 
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shortest  =  expand_paths_with_values (shortest,  expanded, 
minus_tangent_list ,  head_node_list , 
polygon->polygon_number ,  MINUS) ; 

if  (on_same_polygon) 

same_polygon  =  expanded; 

) 

) 

} 

return  (shortest); 


/• 


CHECK  FOR  NODE  ON  POLYGON 

- oOo - 

This  function  determines  if  a  point  is  on  a  polygon.  Since  all  the 
polygons  lie  on  the  same  plane,  depth  was  not  considered  in  this  function 
and  the  z  value  of  the  point  was  not  used.  It  returns  a  one  if  the  node 
is  on  a  polygon,  else  a  zero*/ 


int  chec)c_for_node_on_polygon  (pt,  current) 

point  pt; 

obstacle_plane  ‘current; 

( 

if  (pt.x  ==  current->pt . X  && 

pt.y  ==  current->pt . y  && 

pt.z  ==  current->pt . z) 

return  ( 1 )  ; 

if  (pt.x  ==  current->ccw->pt . X  && 

pt.y  ==  current->ccw->pt . y  && 

pt.z  ==  current->ccw->pt . z ) 

return  ( 1 ) ; 
else 

return  (0)  ; 


/* 


REMOVE  SHORTEST  PATH 

- oOo - 

This  function  removes,  clears  from  memory  the  path  mar)<ed  as  the  shortest. 
In  doing  so,  it  reestablishes  the  linic  list  and  returns  to  the  calling 
function  the  start  of  the  path  list.*/ 


paths 

paths 

( 

paths 

a_path 


*remove_shortest_path ( shortest,  start_path) 
•shortest,  *start_path; 

•current,  ‘previous; 

*current_node,  *previous_node; 


for  (current  =  start_path; 
current  !=  shortest; 

previous  =  current,  current  =  current ->next_path) ; 

if  (current  ==  start_path) 

start_path  =  current->next_path; 
else  ( 

previous->next_path  =  current->next  path; 

> 

current->next_path  =  NULL; 
previous_node  =  current->last_node; 
current_node  =  previous_node; 
free  (current ) ; 
do  ( 

current_node  =  current_node->next_node; 
f ree (previous_node) ; 
prevlous_node  =  current_node; 

)  while  (previous_node  !=  NULL)  ; 
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return  (start_path) ; 


} 

/* 


CROSS  POLYGON  NODE  ON 

- oOo - 

This  function  determines  if  a  line  crosses  the  polygon.  It  checks  each 
line  segment  of  the  polygon  and  returns  1  if  there  is  an  intersection. 
The  first  for  structure  is  used  to  check  if  p2  is  on  a  polygon.  If  so, 
boolean  is  asserted  to  true  (int  value  =  1) .  This  tells  the  rest  of  the 
for  loop  to  go  on  to  the  next  polygon*/ 


int 

polygon_list 

point 

int 


cross_polygon_node_on (start, 
♦start; 
pi,  p2; 
plane_number ; 


int 

polygon_l i st 

obstacle_plane 

point 

double 


count,  boolean  =  FALSE; 
"polygon  =  NULL; 

♦current  =  NULL; 
p3,  p4; 
w,  X,  y,  z; 


Pl,  p2. 


plane_number ) 


for  (polygon  =  start;  polygon  !=  NULL; 
polygon  =  polygon->next_polygon)  ( 


for  (count  =  1,  current  =  polygon->polygon_start ,  boolean  =  FALSE; 
count  <=  polygon->polygon_start->number_nodes; 
count++,  current  =  cur rent->ccw)  { 
if  (polygon->plane_number  !=  plane_number)  { 
boolean  =  TRUE; 
break ; 


if  (p2.x  ==  current->pt . X  &&  p2.y  ==  current->pt .y)  ( 

boolean  =  TRUE; 
break  ; 


) 

if  ( (boolean)  { 

for  (count  =  1,  current  =  polygon->polygon_start ; 

count  <=  polygon->polygon_start->number_noaes; 
count++,  current  =  current->ccw)  { 
p3.x  =  current->pt . X,  p3.y  =  current->pt . y; 
p4.x  =  current->ccw->pt . X,  p4.y  =  cur rent->ccw->pt . y; 

if  ((pl.x  ==  current->pt . X  &&  pl.y  ==  current->pt . y )  II  (pl.x 
==  current->ccw->pt . X  &&  pl.y  ==  current->ccw->pt . y) ) 
continue ; 


if  (point_on_line (current->pt,  current->ccw->pt,  pD)  { 

continue;  /♦  skips  over  pi  which  is  on  the  polygon  ♦/ 

) 


w  =  sign ( functlon_one (p3 . X,  p3.y, 
X  =  sign ( function_one (p4 . X,  p4.y, 
y  =  sign ( function_two (pi . X,  pl.y, 
z  =  sign ( function_two (p2 . X,  p2.y. 


pi,  p2)); 
pi,  p2)); 
p3,  pi ) ) ; 
p3,  pi) ) ; 


if  (w  1=  X  &&  y  :=z) 

return  (1);  /♦  line  intersects  polygon  ♦/ 


) 

current  =  current->ccw; 

) 
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return  (0) ; 

} 


/* 


REVERSE  LIST 

- oOo - 

This  function  identifies  the  shortest  path  and  reverses  it  so  it  is  from 
start  to  goal  vice  goal  to  start  order.  The  function  builds  a  new  path 
in  doing  so  returns  the  path  with  a  pointer  labeled  reverse  path*/ 


paths 

paths 

{ 

paths 

a_path 


*reversed_llst (path_list) 

*path_list; 

*reversed_path; 

*current_path_llst,  *current_reversed_path,  *new_record; 


reversed_path  =  duplicate_entries (path_list) ; 


NEW_NODES (reversed  j3ath->last_node) ; 
current_reversedjpath  =  reversed_path->last_node; 
current_reve.rsed_path->pt  =  assign_polnt_values (start_point) ; 
current_reversed_path->next_node  =  NULL; 

for  (current_path_list  =  path_list->last_node; 
current_path_list  !=  NULL; 

current_path_llst  =  current_path_list->next_node)  ( 


NEW_NODES (new_record) ; 

new_record->pt  =  assign_point_values (current_path_list->pt) ; 
new_record->next_node  =  current_reversed_path->next_node; 
current_reversed_path->next_node  =  new_record; 

) 

return  (reversed_path) ; 


/* 


DIRECTION  HEURISTIC  FROM  START 

- oOo - 

This  function  determines  if  the  new  node  to  be  added  to  the  start  paths 
list  is  valid.  A  node  is  valid  if  the  direction  from  the  start  to  the 
node  is  within  90  degrees  of  the  start  to  goal  direction.  The  initial  pt 
can  either  be  the  start  point  or  the  goal  point.  If  it  is  the  goal  pt 
then  the  goal  direction  is  normalized  to  goal  direction  minus  180  degrees 
*/ 


int  direction_heuristic_from_start (expand,  pt) 

point  expand,  pt; 

{ 

double  ray_direction,  a,  goal_dlr  =  goal_direction; 

if (goal. X  ==  expand. x  &&  goal.y  ==  expand. y  &&  goal.z  ==  expand. z) 
goal_dir  =  norm (goal_dir  +  PI); 

ray_direction  =  atan2((pt.y  -  expand. y),  (pt.x  -  expand. x)); 
a  =  ABS (norm (ray_direction  -  goal_dir) ) ; 
if  (a  <=  HPI) 
return  ( 1 ) ; 
else 

return  (0); 


/* 


DIRECTION  HEURISTIC 

- oOo - 

This  function  determines  if  the  new  node  to  be  added  to  a  valid  path  is 
in  the  correct  direction.  If  the  type  of  tangent  to  the  last  node  is  a 
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plus  tangent,  the  the  new  direction  minus  the  old  direction  must  be  less 
than  or  equal  to  zero.  Vice-a-versa  for  a  minus  tangens*/ 


int 

paths 

point 

{ 

double 

a_path 


direction_heuristic (current,  pt> 

‘current; 

pt; 

ray_dlrection,  a,  goal_dir  =  goal_direction; 
‘node  =  current->last  node; 


if ( ! current->f orward) 

goal_dir  =  norm(goal_dir  +  PI); 

ray_direction  =  atan2((pt.y  -  node->pt.y), 
(pt.x  -  node->pt .X) ) ; 


a  =  ray_direction  -  goal_dir; 
a  =  norm (a) ; 
if  (ABS(a)  >  HPI) 
return  (0); 


a  =  norm (ray_direction  -  current->ray_direction) ; 
if  (ABS(a)  >  PI  /  2) 
return  (0) ; 

/‘  else 

return  (1);‘/ 


if  (current->type_tangent  ==  PLUS)  { 
if  (norm (ray_direction  -  current->ray_direction)  <=  0) 
return  ( 1 ) ; 
else 

return  (0) ; 

) 

else  { 

if  (norm(ray_direction  -  current->ray_direction)  >=  0) 

return  (1); 

else 

return  (0) ; 

) 

) 


NORM 


-OOO- 


This  function  normalizes  an  angle  between  -PI  and  PI.*/ 


double 

double 


norm (a) 
a; 


while  ((a  >  PI)  ||  (a  <=  -PI))  { 

if  (a  >  PI) 
a  =  a  -  DPI; 
else 

a  =  a  +  DPI; 

) 

return  (a) ; 


/* 


UPDATE  CURRENT  PATH 

- OOO - 

This  function  updates  the  path  information  associated  with  the  path 
pointed  at  by  the  current_path  pointer.  After  updating  the  info,  the 
function  returns  the  start_path  pointer  back  to  the  calling  function. ‘/ 


paths 

paths 


‘update_current_path (current_path,  /‘start_path, ‘/  tangent, 
start_pt,  pt,  goal,  polygon_number ) 
‘current_path/‘,  ‘start_path‘/; 
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point 

int 


start_pt,  pt,  goal; 
tangent,  polygon_number; 


current_path->last_node->pt  =  assign_point_values (pt) ; 
current_path->last_node->polygon_number  =  polygon_number ; 
current_path->last_node->next_after_node  =  NULL; 
current_path->cost  =  distance (start_pt,  pt); 
current_path->estimated_cost  =  distance (goal,  pt); 
current_path->total_cost  =  TOTAL_COST; 
current_path->type_tangent  =  tangent; 

current_path->ray_direction  =  atan2((pt.y  -  start_pt.y), 

(pt.x  -  start_pt .x) ) ; 

current_path->plane_number  =  plane_number; 
current_path->able2expand  =  TRUE; 
current_path->forward  =  YES; 

head_active_nodes  =  lnsert_nodes_into_actlve_node_list (head_active_nodes,  pt, 

current_path->cost) ; 

current_path->when_path_found  =  paths_found++; 
return  (current_path) ; 


/* 


EXPAND  PATHS  WITH  VALUES 


- oOo - 

This  function  recalculates  the  header  data  of  the  duplicate  record  of  the 
shortest  path  to  include  the  new  point  to  be  added  (hence  expanded  as  the 
variable  name) .  It  also  inserts  this  expanded  path  into  the  lin)c  list  of 
valid  paths  after  the  shortest  path  and  returns  the  shortest  path  to  the 
calling  function.*/ 


paths  •expand_paths_with_values (shortest,  expanded,  tangent_list, 

head_active,  polygon_number ,  mode) 
paths  ‘shortest,  ‘expanded; 

a_path  ‘tangent_list; 

active_nodes  ‘head_active; 
int  polygon  number,  mode; 

{ 

a_path  ‘current  =  shortest->last_node,  *to_polygon  = 

tangent_list->next_node; 

int  last_tangent_type  =  expanded->type_tangent; 

if  (shortest->last_node->pt.x  ==  to_polygon->pt . x  && 
shortest->last_node->pt .y  ==  to_polygon->pt . y  && 
shortest->last_node->pt . z  ==  to_polygon->pt . z)  { 

expanded->cost  +=  distance (shortest->last_node->pt,  tangent_list->pt) ; 
free  (to_j3olygon)  ; 

tangent_list->next_node  =  expanded->last_node; 
expanded->last_node->next_after_node  =  tangent_list; 
expanded->last  node  =  tangent  list; 

} 

else  { 

expanded  =  connect_llnl<s_of_path_on_polygon (expanded,  tangent_list, 
last_tangent  type); 

} 


/* 

e x p a n d e d - > c o s t  +=  d 1 s t a n c e  ( s h o r t e s t - > 1  a s t _ n o d e - > p t  , 
shortest->last_node->next  node->pt) ; 

*/ 

if (expanded->forward) 

expanded->estimated_cost  =  distance (goal, expanded->last_node->pt) ; 
else 

expanded->estimated_cost  =  distance (start_polnt,  expanded->last_node->pt ) ; 
expanded->total_cost  =  expanded->cost  +  expanded->estimated_cost; 
expanded->ray_direction  =  atan2 ( (expanded->last_node->pt .y  - 

expanded->last_node->next_node->pt . y ) , 
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(expanded->last_node->pt .X  - 
expanded->last_node->next_node->pt . x) ) ; 
expanded->able2expand  =  TRUE; 
expanded->type_tangent  =  mode; 

head_active  =  insert_nodes_into_active_node_list (head_active, 

expanded->last_node->pt,  expanded->cost) ; 


/* 

expanded  =  update (expanded,  mode) ; 

*/ 

expanded->when_path_found  =  paths_found++; 

expanded->next_path  =  shortest->next__path; 
expanded->last_node->polygon_number  =  polygon_number; 
shortest->next_path  =  expanded; 

return  (shortest); 

} 


/* 


ADD  ON  NEW  PLANE  INTERSECTION 

- 000 - 

This  function  finds  the  vertical  and  perpendicular  planes  at  the  point 
passed  in.  The  point  is  the  vertical  point  to  be  expanded  from.  Once 
it  finds  these  planes,  the  function  calls  the  build_two_d_polygon_list, 
which  determines  the  polygons  formed  by  the  intersection  of  the  new 
perpendicular  plane  with  each  polyhedron  in  the  search  space.*/ 


polygon_llst 

polygon_list 

a_path 

point 


*add_on_new_plane_lntersection (start_2d, 

vertical_pt_list , 

*start_2d; 

* vert ical_pt_l 1st; 
goal; 


polygon_list  *last_polygon; 

a_path  ‘current  =  vertlcal_pt_list; 


goal ) 


vertical_n_perpendicular_plane (vertical_pt_list->pt,  goal) ; 
start_2d  =  build_two_d_polygon_list (start_2d,  perpendicular,  TRUE); 


return  (start_2d); 

) 


/* 


POINT  ON  POLYGON 

- 000 - 

This  function  determines  if  a  point  lies  on  a  line.  The  function  is  used 
to  skip  the  polyhedron  edge  which  the  vertical  point  expanded  from  lies 
on.  It  is  also  is  used  as  a  heuristic  to  determine  if  a  new  vertical 
point  lies  between  the  goal  point  and  the  current  point  being  expanded 
on .  •/ 


Int  point_on_line (ptl ,  pt2,  pt3) 

point  ptl,  pt2,  pt3;/*  ptl  and  pt2  are  end  points  of 

*  line,  pt3  is  point  being  checked  */ 

( 

double  tl,  t2,  t3; 

if  (pt2.x  ==  ptl.x)  { 
if  (pt3.x  !=  ptl.x) 
return  (0) ; 
else  { 

if  ((ptl.y  !=  pt2.y)  &&  (ptl.z  !=  pt2.z))  ( 
t2  =  (pt3.y  -  ptl.y)  /  (pt2.y  -  ptl.y); 
t3  =  (pt3.z  -  ptl.z)  /  (pt2.z  -  ptl.z); 
if  (t2  ==  t3) 
return  (1) ; 
else 
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return  (0)  ; 


) 

else  i 

if  (ptl .y  ==  pt2.y)  { 
if  (pt3.y  ==  ptl .y) 
return  (1); 
else  { 

t3  =  (pt3.z  -  ptl.z)  /  (pt2.z  -  ptl.z); 
if  (0  <=  t3  &&  t3  <=  1) 
return  (1); 
else 

return  (0) ; 

) 

) 

else  ( 

if  (ptl.z  ==  pt2.z)  ( 
if  (ptl .z  !=  pt3. z) 
return  (0) ; 
else  ( 

t2  =  (pt3.y  -  ptl.y)  /  (pt2.y  -  ptl.y) 
if  (0  <=  t2  &&  t2  <=  1) 
return  (1); 
else 

return  (0 ) ; 

) 

} 

1 

1 

} 

) 

else  { 

if  (pt2.y  ==  ptl.y)  ( 
if  (pt3.y  !=  ptl.y) 
return  (0); 
else  ( 

if  (ptl .z  !=  pt2 . z)  { 

tl  =  (pt3.x  -  ptl.x)  /  (pt2.x  -  ptl.x); 
t3  =  (pt3.z  -  ptl.z)  /  (pt2.z  -  ptl.z); 
if  (tl  ==  t3) 
return  (1); 
else 

return  (0); 

) 

else  { 

if  (ptl.z  ==  pt2.z)  { 
if  (pt3.z  ==  ptl.z) 

tl  =  (pt3.x  -  ptl.x)  /  (pt2.x  -  ptl.x) 
if  (0  <=  tl  &&  tl  <=  1) 
return  (1); 
else 

return  (0) ; 

} 

) 

) 

} 

else  ( 

if  (ptl.z  ==  pt2.z)  { 
if  (pt3.z  .'=  pt2.z) 
return  (0) ; 

) 

else  { 

tl  =  (pt3.x  -  ptl.x)  /  (pt2.x  -  ptl.x); 
t2  =  (pt3.y  -  ptl.y)  /  (pt2.y  -  ptl.y); 
if  (tl  ==  t2) 
return  (1); 
else  { 

tl  =  (pt3.x  -  ptl.x)  /  (pt2.x  -  ptl.x); 
t2  -  (pt3.y  -  ptl.y)  /  (pt2.y  -  ptl.y); 
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t3  =  (pt3.z  -  ptl.z)  /  (pt2.z  -  ptl.z); 
if  (tl  ==  t2  ==  t3) 
return  (1); 
else 

return  (0); 

} 

} 

} 

} 

return  (0) ; 


/* 


CONNECT  LINKS  OF  PATH  ON  POLYGON 

- oOo - 

This  function  connects  the  links  of  a  path  on  the  polygon.  The  tangent 
function  finds  the  either  a  plus  or  minus  tangent  between  polygons.  This 
tangent  connects  the  path  between  the  last  node  of  the  expanded  path  and 
the  tangent  node  which  lies  on  the  same  polygon  as  that  last  node.*/ 


paths 

paths 

a_path 

int 


*connect_links_of_path_on_polygon (expanded,  tangent_list , 
‘expanded; 

*tangent_list; 
mode ; 


a_path  *last_node_in_tangent  =  tangent_list->next_node; 

polygon_list  ‘polygon; 
obstacle_pl?ne  ‘node; 
point  extend; 


mode) 


extend  =  expanded->last_node->pt; 

last_node_in_tangent->polygon_number  =  expanded->last_node->polygon_number ; 

/‘  finds  correct  polygon  points  lie  on  ‘/ 
for  (polygon  =  start_2d; 

polygon->polygon_number  !=  expanded->last_node->polygon_number; 
polygon  =  polygon->next_polygon) ; 


/‘ 

‘  finds  the  correct  node  in  polygon  which  is  equal  to  last  node  in 
‘  tangent  list 

‘/ 

for  (node  =  polygon->polygon_start; 

node->pt.x  !=  last_node_in_tangent->pt.x  II 
node->pt.y  !=  last_node_in_tangent->pt .y  II 
node->pt.z  !=  last_node_in_tangent->pt . z; 
node  =  node->ccw) ; 

while  (LOOPFOREVER)  ( 
if  (mode  ==  PLUS)  ( 

if  (extend. X  •==  node->cw->pt .x  S&  extend. y  ==  node->cw->pt .y  &&  extend. z  == 
node->cw->pt . z) 
break; 

else  ( 

NEW_NODES (last_node_in_tangent->next_node) ; 

last_node_in_tangent->next_node->next_after_node  =  last_node_in_tangent; 
last_node_in_tangent  =  last_node_in_tangent->next_node; 
last_node_in_tangent->pt  =  assignjpoint_values (node->ccw->pt) ; 

node  =  node->ccw; 

) 

else  ( 

if  (extend. X  ==  node->ccw->pt .x  &&  extend. y  ==  node->ccw->pt .y  &&  extend. z 
==  node->ccw->pt . z) 
break; 
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else  { 

NEW_NODES  <last_node_in_tangent->next_node) ; 

last_node_ln_tangent->next_node->next_after_node  =  last_node_in_tangent  ; 
last_node_in_tangent  =  last_node_ln_tangent->next_node; 
last_node_in_tangent->pt  =  asslgn_point_values (node->cw->pt) ; 

} 

node  =  node->cw; 

} 

} 

expanded->cost  =  calculate_cost (tangent_list,  expanded->last_node, 

expanded->cost,  last_node_in_tangent) ; 
last_node_in_tangent->next_node  =  expanded->last_node; 
expanded->last_node->next_after_node  =  last_node_in_tangent; 
expanded->last_node  =  tangent_list; 
return  (excanded) ; 

} 


double 

a_path 

double 

a_path 


calculate_cost (list,  last_node_ln_path,  cost,  last_tangent) 
‘list,  *last_node_in_path,  *last_tangent; 
cost; 

‘tangent  =  last_tangent; 


cost  +=  distance (last_node_ln_path->pt,  last_tangent->pt ) ; 
head_active_nodes  =  insert_nodes_into_active_node_list (head_active_nodes, 

last_tangent->pt,  cost) ; 


for  (tangent  =  last_tangent->next_after_node; 
tangent  !=  NULL; 

tangent  =  tangent ->next_after_node)  { 
cost  +=  distance (tangent->pt,  tangent->next_node->pt) ; 

head_active_nodes  =  insert_nodes_into_active_node_list (head_active_nodes, 

tangent->pt,  cost) ; 


} 

return  (cost); 


/* - oOo- 

MARK  SHORTEST  LAST  NODE 


This  function  marks  the  last  node's  polygon  number  with  the  polygon's 
number  in  which  the  node  was  found. 

*/ 

paths  ‘mark_shortest_path_last_node (shortest,  start2) 
paths  ‘shortest; 
polygon_list  ‘start2; 

( 

polygon_list  ‘polygon; 
obstacle_plane  ‘node; 
int  count; 

for  (polygon  =  start2; 
polygon  !=  NULL; 

polygon  =  polygon->next_polygon)  { 

if  (shortest->plane_number  !=  polygon->plane_number) 
continue; 

for (count  =  1,  node  =  polygon->polygon_start; 
count  <=  polygon->polygon_start->number_nodes; 
node  =  node->ccw,  count ++) ( 

if  (node->pt.x  !=  shortest->last_node->pt .x  II 
node->pt.y  !=  shortest->last_node->pt .y  II 
node->pt.z  !=  shortest->last_node->pt .z) 
continue; 
else  { 
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shortest->last_nocie->polygon_number  =  polygon->polygon_number ; 
break; 

} 

) 

) 

return (shortest) ; 


) 

/. - 

UPDATE 

- 000 - 

This  function  accepts  a  path  which  has  been  expanded  on  and  updates 
the  header  information  accordingly,  passing  back  the  path  and  updated 
header  info 
*/ 


paths  ‘update (the_path,  tan) 
paths  *the_path; 
int  tan; 

( 

the_path->cost  +=  distance (the_path->last_node->pt, 
the_path->last_node->next_node->pt) ; 
the_path->estimated_cost  =  distance (the_path->last_node->pt,  goal); 
the_path->total_cost  =  the_path->cost  +  the_path->estimated_cost; 

the_path->ray_direction  =  atan2 ( (the_path->last_node->pt . y  - 

the_path->last_node->next_node->pt . y ) , 
(the_path->last_node->pt .X  - 
the__path->last_node->next_node->pt .x) ) ; 
thejpath->able2expand  =  TRUE; 
the_path->type_tangent  =  tan; 

head_active_nodes  =  lnsert_nodes_into_active_node_list (head_active_nodes, 

the_path->last_node->pt,  the_path->cost) ; 

thejpath->when_path_found  =  paths_found++; 

the_path->next_path  =  the  jpath->next_path; 

return (the_path) ; 


int  dlrection_heuristic4tangent (path, list) 
paths  ‘path; 
a_path  ‘list; 

{ 

double  direction,  goal_dir  =  goal_direction,  a; 
a_path  ‘node  =  path->last_node; 

if ( !path->forward) 

goal_dir  =  norm (goal_dir  +  PI); 

direction  =  atan2 ( (list->pt .y  -  list->next_node->pt .y) , 
(list->pt.x  -  list->next_node->pt .x) ) ; 

a  =  direction  -  goal_dir; 
a  =  norm (a) ; 
if  (ABS(a)  >  HPI) 
return  (0); 

if (node->pt .X  ==  list->pt.x  && 
node->pt.y  ==  list->pt.y  ss 
node->pt.z  ==  list->pt.z){ 

a  =  norm (path->ray_dlrection  -  direction); 
if(ABS(a)  >  HPI) 
return  (0)  ; 
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} 

return(l) 


APPENDIX  G 


This  appendix  contains  the  source  code  to  make  the  simple  plots  of  the  world.  This 
source  code  is  DOS  dependent  and  Is  not  Included  If  the  defined  variable  DOS  is 
equal  to  1  during  compilation. 


#lnclude  "3d_tan.h" 
# Include  "plot.h" 

#lfdef  DOS 


ideflne  ESC 
^define  SCALEDX 
^define  SCALEDY 
#define  DOT 


Oxlb/*  Define  the  escape  key*/ 

X  /  48 
y  /  34 

1  /*used  to  make  a  dot  with  radius  1*/ 


void  data_plot(start_2d) 
polygon_list  *start_2d; 

( 

Int  X,  y,  z,  w; 


Inltlallze( ) ; 

MainWlndow(  "PLOT  OF  THE  TWO  D  ENVIRONMENT"  ); 


StatusLlne(  "PRESS  ANY  KEY  TO  END"  ); 

X  *  getmaxxO; 
y  -  getmaxyO; 

z  -  start_point.x  *  SCALEDX;  w  =  start_point . y  *  SCALEDY; 

outtextxy(  z,  w,  "X"  ); 

outtextxy(z+20,  w+15,  "START"); 

z  -  goal.x  *  SCALEDX;  w  =  goal.y  *  SCALEDY; 

outtextxy(  z,  w,  "X"  ); 

outtextxy(z  +  20,  w+15,  "GOAL"); 

draw_boundry ( ) ; 

draw_polygons ( star t_2d ) ; 


) 


void  Initlallze(vold) 

( 

int  xasp,  yasp;  /*  Used  to  read  the  aspect  ratio*/ 

if (registerbgidriver (EGAVGA_driver )  <  0)  /*checks  for  correct  driver*/ 
exit(l) ; 

if (regi8terbglfont(triplex_font)  <  0)  /*checks  for  correct  font*/ 
exlt(l) ; 

GraphDrlver  -  DETECT;  /*  Request  auto-detection*/ 

inltgraph(  tiGraphDrlver ,  &GraphMode,  *"); 

ErrorCode  -  graphresult( ) ;/*  Read  result  of  initialization*/ 

if(  ErrorCode  !”  grOk  ){/*  Error  occured  during  init*/ 

printf{"  Graphics  System  Error:  %s\n",  grapherrormsg(  ErrorCode  )  ); 
exit(  1  ); 

) 

getpalette(  fipalette  );  /*  Read  the  palette  from  board*/ 

MaxColors  -  getmaxcolor ( )  +  1;/*  Read  maximum  number  of  colors*/ 

MaxX  -  getmaxx(); 

MaxY  -  getmaxy();  /*  Read  size  of  screen*/ 
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getaapectratlo(  &xasp,  &yasp  );/*  read  the  hardware  aspect*/ 
AspectRatio  =  (double)xasp  /  (double)yasp;  /*  Get  correction  factor*/ 


/*  V 

/*  PAUSE:  Pause  until  the  user  enters  a  keystroke.  If  the*/ 
/*  key  is  an  ESC,  then  exit  program,  else  simply  return.*/ 

/♦  */ 

void  Pau8e{void) 

t 

static  char  msg[]  =  "Pausing.  Esc  aborts  or  press  a  key..."; 
int  c; 

StatusLlne(  msg  );  /•  Put  msg  at  bottom  of  screen*/ 

c  -  getch();  /*  Read  a  character  from  kbd*/ 

if{  ESC  "  c  ) {  /*  Does  user  wish  to  leave?*/ 

closegraph( ) ;  /*  Change  to  text  mode*/ 

exlt(  1  ) ;  /*  Return  to  OS  */ 

1 

if(  0  c  ) {  /*  Did  use  hit  a  non-ASCII  key?  */ 

c  “  getch();  /*  Read  scan  code  for  keyboard*/ 

1 

StatusLlne( "Working  on  next  expansion"); 

/*cleardevice( ) ; */  /*  Clear  the  screen*/ 


void  MainWlndow(  char  *header  ) 

I 

int  height; 

cleardevlce{ ) ;  /*  Clear  graphics  screen*/ 

setcolor(  MaxColors  -  1  );/*  Set  current  color  to  white*/ 
setviewport(  0,  0,  MaxX,  MaxY,  1  );/*  Open  port  to  full  screen*/ 

height  -  textheight(  *H"  );  /*  Get  basic  text  height  */ 

changetext8tyle(  DEFAULTFONT ,  HORIZDIR,  1  ); 
settextjustify(  CENTER_TEXT,  TOPTEXT  ); 
outtextxy{  MaxX/2,  2,  header  ); 

8etviewport(  0,  height+4,  MaxX,  MaxY- (height+4 ) ,  1  ); 

DrawBorder( ) ; 

setvlewport(  1,  height+5,  MaxX-1,  MaxY- {height+5 ) ,  1  ); 


/*  V 

/*  DRAWBORDER:  Draw  a  solid  single  line  around  the  current  */ 
/*  viewport.  */ 

/*  V 

void  DrawBorder(vold) 

{ 

struct  viewporttype  vp; 

setcolor(  MaxColors  -  1  );/*  Set  current  color  to  white*/ 
8etline8tyle(  SOLIDLINE,  0,  NORMWIDTH  ); 
getvlewsettlngs(  svp  ); 

rectangle(  0,  0,  vp . right-vp. left,  vp . bottom -vp . top  ); 
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) 

/*  V 

/*  STATUSLINE:  Display  a  status  line  at  the  bottom  of  the  screen.*/ 

/*  */ 

void  StatusLine(  char  *msg  ) 

{ 

int  height; 

setvtewport(  0,  0,  MaxX,  MaxY,  1  );/•  Open  port  to  full  screen*/ 
8etcolor(  MaxColors  -  1  );/*  Set  current  color  to  white*/ 

changetextstylef  DEFAULTFONT,  HORIZDIR,  1  ); 
settextjustifyC  CENTERTEXT,  TOPTEXT  ); 

8etlinestyle(  SOLID_LINE,  0,  NORMWIDTH  ); 

8etfill8tyle(  EMPTY_FILL,  0  ); 

height  -  textheight(  "H"  );  /*  Detemine  current  height  */ 

bar(  0,  MaxY-(height+4),  MaxX,  MaxY  ); 
rectangle(  0,  MaxY- (height+4 ) ,  MaxX,  MaxY  ); 
outtextxy(  MaxX/2,  MaxY- (height+2) ,  msg  ); 

8etviewport(  1,  hetght+5,  MaxX-1,  MaxY- (height+5) ,  1  ) ; 


1 

void  changetextstyle( int  font,  int  direction,  int  charsize) 

I 

int  ErrorCode; 

graphre8ult( ) ;  /*  clear  error  code*/ 

8ettextstyle( font,  direction,  charsize); 

ErrorCode  -  graphresult ( ) ;/*  check  result  */ 
if(  ErrorCode  I-  grOk  )(/*  if  error  occured*/ 
closegraph( ) ; 

printf("  Graphics  System  Error;  %s\n",  grapherrormsg (  ErrorCode  )  ); 
exit(  1  ); 

1 

) 


void  draw_boundry ( ) 
I  int  X,  y; 


) 


X  -  getinaxx( ) 
y  -  getmaxyo 
setlinestyleC 
8etcolor(4) ; 
line(boundry . 

boundry . 
line(boundry . 

boundry . 
line (boundry . 

boundry . 
line (boundry. 
boundry . 


SOLIDLINE,  0, 

Xl  *  SCALEDX, 
y2  *  SCALEDY) 
Xl  *  SCALEDX, 
yl  *  SCALEDY) 
x2  *  SCALEDX, 
y2  *  SCALEDY) 
xl  *  SCALEDX, 
y2  *  SCALEDY) 


THICK  WIDTH); 


boundry . yl 
boundry . yl 
boundry . yl 
boundry . y2 


*  SCALEDY, 

*  SCALEDY, 

*  SCALEDY, 

*  SCALEDY, 


boundry. xl  *  SCALEDX, 
boundry. x2  *  SCALEDX, 
boundry. x2  *  SCALEDX, 
boundry. x2  *  SCALEDX, 


void  draw_polygons(start_2d) 
polygon_li8t  *8tart_2d; 

I 

polygonlist  *poly; 
obstacle_plane  ‘node; 
int  count  •  1,  X,  y; 

X  -  getmaxx(); 
y  ”  getmaxyo; 
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8etline8tyle(S0LID_LINE,  0,  NORMWIDTH) ; 

for  (fJoly  -  start_2d;  poly  1=  NULL;  poly  =  poly->next_polygon) ( 
for  (node  -  poly->polygon_8tart,  count  ”  1; 

count  <-  poly->polygon_start->number_nodes; 
node  »  node->ccw,  count++){ 


) 


line( node->pt . X  *  SCALEDX,  node->pt.y  *  SCALEDY, 

node->ccw->pt.x  *  SCALEDX,  node->ccw->pt . y  *  SCALEDY); 


void  draw_8horte8t_path ( shortest ) 
paths  *8hortest; 

{ 

paths  *path; 

a_path  *polnt; 

int  X,  y,  w,  z,  style; 

X  -  getmaxx ( ) ; 
y  -  getmaxy ( ) ; 
style  •  THICKWIDTH; 
setcolor (14 )  ; 

8etlinestyle(S0LID_LINE,  0,  THICKWIDTH) ; 
for(point  *•  8hortest->laat_node;  point  !=  NOLL; 
point  -  polnt->next_node) [ 
if (polnt->next_node  I-  NULL) 

draw_llne_segment(point->pt,  point->next_node->pt,  YELLOW,  style); 

/*( point- >pt.x  *  SCALEDX,  point- >pt.y  *  SCALEDY, 

point- >next_node->pt.x  *  SCALEDX,  point->next_node->pt . y  •  SCALEDY);*/ 
clrcle(polnt->pt .X  •  SCALEDX,  point->pt.y  *  SCALEDY,  DOT); 

) 

Pau8e( ) ; 
cleardevlce( ) ; 


void  draw_llne_segment(ptl,  pt2,  color,  style) 
point  ptl,  pt2; 
int  color,  style; 

( 

Int  X,  y,  w,  z; 

X  “  getmaxx ( ) ; 
y  -  getmaxyO; 
setcolor (color ) ; 
if (color  — •  RED  II  style) 

8etlinestyle(S0LID_LINE,  0,  THICKWIDTH) ; 
else 

8etlinestyle(D0TTED_LINE,  0,  NORMWIDTH) ; 
llne(pt2.X  *  SCALEDX,  pt2.y  *  SCALEDY,  ptl.x  *  SCALEDX,  ptl.y  *  SCALEDY); 
z  -  ptl.x  •  SCALEDX;  w  =  ptl.y  ♦  SCALEDY; 
outtextxy(  z,  w,  "X"  ); 

z  -  pt2.x  •  SCALEDX;  w  -  pt2.y  ♦  SCALEDY; 
outtextxy(  z,  w,  "X*  ); 


void  id_8hortest_path_for_expans ion (shortest) 
paths  ‘shortest; 

( 


int  x,  y,  color,  1; 
point  pi,  p2; 
a_path  ‘node; 

X  -  getmaxx ( ) ; 
y  -  getmaxyO; 


124 


setcolor ( BLACK ) ; 

outtextxy( Expans ion_lable_x  *  SCALEDX, 

Expans ion_lable_y  *  SCALEDY,  "X"  ); 
outtextxy( Expans ion_lable_x  *  SCALEDX  +  20, 

Expans ion_lable_y  *  SCALEDY  +  15,  "EXPAND"); 

for(l  -  1,  node  =  shortest- >last_node; 
node->next_node  1=  NULL; 
node  =  node->next_node,  !++)( 
pi  -  node->pt; 
p2  -  node->next_node->pt; 

draw_line_segment (pi ,  p2,  RED,  REGULAR); 

if(i  -=  1)( 

Expanslon_lable_x  =  pl.x; 

Expans ion_lable_y  =  pl.y; 

outtextxy(  pl.x  *  SCALEDX,  pl.y  *  SCALEDY,  "X"  ); 
outtextJ y(pl .X  *  SCALEDX  +  20,  pl.y  *  SCALEDY  +  15, 

) 

) 

Pause ( ) ; 


) 

#endif 


"EXPAND" ) ; 
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APPENDIX  H 


This  appendix  contains  the  source  code  in  print. c  and  has  only  those  functions 
associated  with  printing  to  either  a  file  or  the  screen.  It  also  contains  some 
print  utilities  I  used  in  debugging  other  portions  of  my  code 

♦include  "3d_tan.h" 

void  print_polyhedron (list) 

obstacle_list  *list; 

obstacle_llst  *current_obstacle  =  polyhedron_list; 
polyhedron_obstacle  *current_face; 
polyhedron_obstacle_plane  ‘current; 
int  countl,  count2,  counts ; 

for  (countl  =  1,  current_obstacle  =  list; 
countl  <=  number_polygons; 
countl++, 

current  obstacle  =  current_obstacle->next_polygon)  { 
printf r"\nObstacle  %d\n",  countl); 

for  (current_face  =  current_obstacle->obstacle,  count2  =  1; 
current_face  !=  NULL; 

current_face  =  current_face->next_face,  count2++)  { 
printf ("\nplane  face  %d\n",  count2); 
for  (current  =  current_f ace->face_nodes,  counts  =  1; 

counts  <=  current_f ace->face_nodes->number_nodes; 
countS++,  current  =  current->ccw)  ( 

printf("\nx  =  %.Slf,  y  =  %.Slf,  z  =  %.Slf", 

current->pt .X,  current->pt .y ,  current->pt . z) ; 

} 

} 

} 

) 

/* - 

PRINT  RESULTS 

- oOo - */ 

void  print_results (polyhedron_list,  number_polygons, 

startjpoint,  goal,  plane_equation) 
obstacle_list  *polyhedron_list; 
equation  plane_equation; 

point  start_point,  goal; 

{ 

int  countl,  count2,  counts; 

obstacle_llst  *current_obstacie  =  polyhedron_] ist; 
polyhedron_obstacle  *current_face; 
polyhedron_obstacle_plane  ‘current; 

polyhedron_obstacle_plane  ‘ptl  =  NULL,  *pt2  =  NULL,  ‘ptS  =  NULL; 

for  (countl  =  1,  current_obstacle  =  polyhedron_list; 
countl  <=  number_polygons; 
countl++, 

current_obstacle  =  current_obstacle->next_polygon)  ( 
printf ( "\nObstacle  %d\n",  countl); 

for  (current_face  =  current_obstacle->obstacle,  count2  =  1; 
current_face  !=  NULL; 

current_face  =  current_face->next_face,  count2++)  { 
printf ("\nplane  face  %d\n",  count2); 
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for  (current  =  current_face->f ace_nodes,  counts  =  1; 

counts  <=  current_face->face_nodes->number_nodes; 
countS++,  current  =  current->ccw)  ( 

printf("\nx  =  %.Slf,  y  =  %.Slf,  z  =  %.Slf", 

current->pt . X,  current->pt .y,  current->pt . z) ; 

> 

} 


/*  check  */ 

printf ("\nstart  point  is  %.Slf,  %.Slf,  %.Slf\n", 
start_point .X,  start_point.y,  start  point . z > ; 
printf  ("Nngoal  is  %.Slf,  %.Slf,  %.Slf\n’*, 
goal.x,  goal.y,  goal.z); 

/*  check  plane  equation  for  first  face  •/ 
current_face  =  polyhedron_list->obstacle; 
for  (count2  =  1,  current_obstacle  =  polyhedron_list; 
current_obstacle  1=  NULL; 

current_obstacle  =  current_obstacle->next_polygon, 
current_face  =  polyhedron_list->obstacle)  ( 

for  (countl  =  ? ,  current_face  =  current_obstacle->obstacle; 
current_face  (=  NULL; 

current_face  =  current_face->next_faca,  countl++)  { 

printf ("face  %d\n",  countl); 
ptl  =  current_face->face_nodes; 
pt2  =  ptl->ccw; 
ptS  =  pt2->ccw; 

printf ("\nx  =  %.Slf,  y  =  %.Slf,  z  =  %.Slf", 

ptl->pt.x,  ptl->pt.y,  ptl->pt.z); 
printf("\nx  =  %.Slf,  y  =  %.Slf,  z  =  %.Slf", 

pt2->pt.x,  pt2->pt.y,  pt2->pt.z); 
printf ("\nx  =  %.Slf,  y  =  %.Slf,  z  =  %.Slf", 

ptS->pt.x,  ptS->pt.y,  ptS->pt.z); 

plane_equation  =  f ind_plane_equation (ptl->pt,  pt2->pt, 

ptS->pt) ; 

printf ("plane  equation  =  <%.Sf,  %.Sf,  %.Sf,  %.Sf>\n", 
plane_equation . x,  plane_equation. y, 
plane_equation. z,  plane  equation. d); 

) 

) 

) 


/* 


PRINT  2D  PATH 
- oOo - 


void  print_2d_path (start_two_d) 

polygon_list  "start  two  d; 

{ 

polygon_list  *current_polygon  =  start_two_d; 

obstacle_plane  "current; 

int  countl,  count2; 

for  (countl  =  1;  current_polygon  !=  NULL; 

current_polygon  =  current_polygon->next_polygon,  countl++)  { 
if  (current_polygon->polygon_start  ==  NULL) 
continue; 


printf ( "\nPOLYGON  %d\nprint  ccw\n",  countl); 

for  (count2  =  1,  current  =  current_polygon->polygon_start ; 
count2  <=  current_polygon->polygon_start->number_nodes ; 
count2++,  current  =  current->ccw)  ( 


"/ 
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} 

/* 


printfC'x  coord  =  %8.31f,  y  coord  =  %8.31f,  z  coord  =  %8.31f\n", 
current->pt .X,  current->pt . y,  current->pt . z ) ; 

) 

printf ("print  cw\n"); 

for  (count2  =  1,  current  =  current_polygon->polygon_start; 
count2  <=  current_polygon->polygon_start->number_nodes; 
count2++,  current  =  current->cw)  ( 
printfC'x  coord  =  %8.31f,  y  coord  =  %8.31f,  z  coord  =  %8.31f\n", 
current ->pt .X,  current->pt .y,  current->pt . z) ; 

) 

) 


PRINT  VERT  PERP  PATH 
- oOo - 


void  print_vert_perp_plane () 

{  /*  all  variables  are  globals  */ 

printf  C'SnThe  line  intersects  the  polygon  at  (%.3f,  %.3f,  %.3f)", 
intersection. X,  intersection.y,  intersection . z) ; 
printf ("\nThe  vertical  plane  through  the  start  and  goal  is\n"); 
printf  ("%.3f I  +  %.3fJ  +  %.3fK  *  %.3f", 

vertical. X,  vertical. y,  vertical. z,  vertical. d); 
printf  ("\nThe  plane  through  the  start  and  goal  perpendicular  to  the"); 
printf  ("  vertical  plane  is\n  %.'3fl  +  %.3fJ  +  %.3fK  *  %.3f", 

perpendicular .X,  perpendicular .y, 
perpendicular . z,  perpendicular. d) ; 


*/ 


/*- 


PRINT  EACH  PATH 


*/ 


void 

paths 

{ 

paths 

int 


prlnt_each_path (start_paths,  iteration_count) 
*start_paths; 

•current_path; 

count; 


if  ( !plot2screen  II  print2file)  { 
if  (print2file) 
if(l  ==  iteration_count) 

fprintf (fpt2,  "\n  INITIAL  PARTIAL  PATHSXn")  ; 
else 

fprintf (fpt2,  "\n\nPATH  FINDING  ITERATION  COUNT  =  %d",  iteratlon_count++) ; 
for  (current_path  =  start_paths,  count  =  1; 
current_path  !=  NULL; 

count++,  current_path  =  current_path->next_path)  { 

if  ( !plot2screen) 

printf ( "\n\nPATH  %d\n",  count); 
if  (prlnt2file) 

fprintf (fpt2,  "\n\nPATH  %d\n",  count); 

print_path (current  jpath) ; 

) 

) 

} 


/* - 

PRINT  INTERSECTION 

- oOo - */ 

void  print_intersection (plane_equation,  intersection,  ptl,  pt2) 

equation  plane_equation; 

point  ptl,  pt2.  Intersection; 

{ 

printf  ("\nThe  intersection  of  the  line,"); 
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printf ("\n (% . 3f , % . 3f ,  %.3f)  to  (%.3f,%.3f,  %.3f)\nand  the  plane", 
ptl.x,  ptl.y,  ptl.z,  pt2.x,  pt2.y,  pt2.z); 
printf ("  (%.3fl  +  %.3fJ  +  %.3fK  +  %.3f)  is  (%.3f,  %.3f,  %.3f)", 
plane_equation . x,  plane_equation .y, 
plane_equation . z,  plane_equation.d, 
intersection . X,  intersection. y,  intersection . z) ; 


/* 


PRINT  PATH 


oOo 


void  print_path (current_path) 

paths  *current_path; 

{ 

a_path  *node; 

if  { !plot2screen)  { 

printf ("PATH  NUMBER  %d  ",  current_path->when_path_f ound) ; 
if (current_path->able2expand) 

printf ("Path  able  to  extendXn"); 
else 

printf ("Path  cannot  be  extended\n") ; 

if  (current_path->type_tangent  ==  PLUS) 
printf  ("\nPlus  Tangent,  direction  =  %4.31f\n", 
current_path->ray_direction) ; 

else 

printf ( "\nMinus  Tangent,  direction  =  %4.31f\n", 
r"rrent_path->ray_direction) ; 
for  (node  =  current_path->last_node; 
node  !=  NULL; 
node  =  node->next_node)  { 

if  (node->next_node  !=  NULL)  { 

printf ("(%3.21f,  %3.21f,  %3,21f)  to", 

node->pt.x,  node->pt.y,  node->pt.z); 
printf ("\ncost  =  %4.31f,  est.cost  =  %4.31f,  total  =  %4.31f", 
current_path->cost ,  current_path->estiniated_cost , 
current_path->total_cost) ; 


*/ 


) 

else  { 

printf (" (%3.21f,  %3.21f,  %3.21f)", 

node->pt.x,  node->pt.y,  node->pt.z); 
printf ( "\ncost  =  %4.31f,  est.cost  =  %4.31f,  total  =  %4.31f", 
current_path->cost,  current_path->estlrnated_cost , 
current_path->total_cost) ; 

) 

) 

) 

if  (print2file) 

print_path_to_f lie (current_path) ; 


) 


void 

paths 


print  jpath_to_fi le (current_path) 
*current_path; 


{ 

a_path 


•node; 


fprlntf (fpt2,  "PATH  NUMBER  %d  ",  current_path->when_path_four,d) ; 
if (current_path->able2expand) 

fprintf (fpt2,  "Path  able  to  extend\n"); 
else 

fprintf (fpt2,  "Path  cannot  be  extendedXn") ; 
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fprintf (fpt2,  "cost  =  %4.31f,  est.cost  =  %4.31f,  total  =  %4.31f\n", 
current_path->cost ,  current_path->estimated_cost , 
current_path->total_cost )  ; 

if  (current_path->type_tangent  ==  PLUS) 

fprintf (fpt2,  "\nPlus  Tangent,  direction  =  %4.31f\n", 
current_path->ray_direction)  ; 

else 

fprintf (fpt2,  "\nMinus  Tangent,  direction  =  %4.31f\n", 
current_path->ray_direction)  ; 
for  (node  =  current_path->last_node; 
node  !=  NULL; 
node  =  node->next_node)  { 
if  (node->next_node  !=  NULL)  { 
fprintf (fpt2,  "(%3.21f,  %3.21f,  %3.21f)  to\n", 
node->pt.x,  node->pt.y,  node->pt . z ) ; 

) 

else  { 

fprintf (fpt2,  "(%3.21f,  %3.21f,  %3.21f)\n", 
node->pt.x,  node->pt.y,  node->pt.z) ; 

) 

) 

) 

/* - 

PRINT  PATH  SHORTEST 

- oOo - */ 

void  print_patli_shortest  (shortest) 

paths  * shortest; 

{ 

a_path  ‘current; 

if  ( !plot2screen) 

printf ("\nSHORTEST  PATH"); 
if  (print2file) 

fprintf (fpt2,  "SHORTEST  PATH\n"); 

for  (current  =  shortest->last_node; 
current  !=  NULL; 
current  =  current->next_node)  { 
if  ( !plot2screen)  ( 
printf  ("\n(%3,21f,  %3.21f,  %3.21f)", 

current->pt .X,  current->pt . y,  current->pt . z) ; 
if  (current->next_node  !=  NULL) 
printf ("  to"); 

) 

if  (print2file)  { 

fprintf (fpt2,  "\n(%3.21f,  %3.21f,  %3.21f)", 

current->pt.x,  current ->pt .y,  current->pt . z) ; 
if  (current->next_node  !=  NULL) 
fprintf (fpt2,  "  to"); 

) 

) 

if  ( !plot2screen) 

printf ("\nThe  total  length  of  the  shortest  path  is  %4.31f\n", 
shortest ->total_cost) ; 
if  (print2file) 

fprintf (fpt2,  "\nThe  total  length  of  the  shortest  path  is  %4.31f\n", 
shortest->total  cost) ; 
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APPENDIX  I 


This  function  contains  the  source  code  found  in  tangent. c  and  contains  only  those 
functions  which  are  associated  with  tangents. 

#include  "3d_tan.h" 

- oOo - 

TANGENT 

- oOo - 

This  function  finds  the  tangent  if  one  exist  between  two  polygons.  If  any 
obstacles  lie  between  the  two  polygons,  then  the  function  returns  NULL.  If  a 
tangent  line  is  found  then  the  points  are  placed  in  a  list  in  euclidean  order  and 
returned. 

*/ 

a_path  *tangent{8hortest,  polygon,  8econd_polygon_start,  mode, 

start_2d,  type_path,  mode2) 
paths  *shortest; 

polygon_llst  ^polygon; 
obstacle_plane  *second_polygon_start; 
int  mode; 

polygon_llst  *start_2d; 

int  type_path; 

int  mode2; 

{ 

a_path  *tangent_path_list  =  NULL; 

point  to_polygon_pt,  from_polygon_pt; 

int  i,  points_found  =  0,  tan; 

if (type_path) 

tan  -  8hortest->type_tangent; 
else 

tan  =  shortest->polygon4vert_PTR->type_tangent; 

if (type_path)  mode2  »  shortest- >type_tangent; 

if  (mode2  /*shortest->type_tangent*/  ==  MINUS)  { 
if  (mode  MINUS)  ( 

for(i  =  1; 

i  <=  8econd_polygon_start->number_nodes  && 
i  <=  polygon->polygon_start->number_nodes; 

i++)l 

to_polygon_pt  =  minus_tangent{second_polygon_start, 
8hortest->last_node->pt) ; 

from_polygon_pt  =  plus_tangent(polygon->polygon_start,  to_polygon_pt) ; 
if  { lcross_polygon_node_on(8tart_2d,  to_polygon_pt,  from_polygon_pt, 
polygon->plane3iumber) ) { 
points_found  -  TRUE; 

break; 

) 

) 

1 

else  ( 

for(i  -  1; 

1  <«  8econd_polygon_start->number_nodes  66 
1  <«  polygon->polygon_8tart->number_nodes; 

!++)( 

to_polygon_pt  «  plus_tangent(second_polygon_start, 
shortest- >last_node->pt) ; 

from_polygon_pt  -  plu8_tangent(polygon->polygon_start,  to_polygon_pt) ; 
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if  ( !cross_polygon_node_on(start_2d,  to_polygon_pt,  from_polygon_pt, 
polygon  -  >plane_nuniber ) )  { 
polnts_found  =  TRUE; 

break; 

) 

) 

1 

) 

else  { 

if  (mode  ==  MINUS)  { 
for(i  -=  1; 

1  <»  second_polygon_start->number_nodes  && 

1  <=  polygon- >polygon_start->nuinber_nodes; 

!++){ 

to_polygon_pt  ”  minus_tangent(second_polygon_start, 

shortest->la8t_node->pt) ; 

from_polygon_pt  =  mlnus_tangent{polygon->polygon_start,  topolygonpt) ; 
if  ( lcross_polygon_node_on(start_2d,  to_polygon_pt,  frompolygonpt, 
polygon->plane_nuinber) )  ( 
points_found  -  TRUE; 

break; 

) 

) 

] 

else  ( 

for ( i  =  1 ; 

1  <=  second_polygon_start->number_nodes  && 

1  <=  polygon->polygon_start->number_nodes; 

i++){ 

to_polygon_pt  -  plus_tangent(second_polygon_8tart, 

8horte8t->la8t_node->pt) ; 

from_polygon_pt  -  minu8_tangent(polygon->polygon_8tart,  to_polygon_pt) ; 
if  ( lcross_polygon_node~on(start_2d,  to_polygon_pt ,  from_polygon_pt, 
polygon->plane_nuinber) )  { 
points_found  =  TRUE; 

break; 

) 

1 

) 

] 

if (pointsfound) 

tangent_path_list  =  make_tangent_list(to_polygon_pt,  from_polygon_pt) ; 
return  (tangent_path_list) ; 


a_path 

point 


*make_tangent_list( topoint,  from_point) 
to_point,  frompoint; 


( 

apath 


♦list; 


NEW_N0DES(li8t); 
NEW_N0DES(li8t->next_node) ; 
li8t->next_node->next_node  =  NULL; 
li8t->next_node->next_after_node  -  list; 
li8t->next  after  node  «  NULL; 


li8t->pt  “  as8ign_potnt_value8(to_point) ; 
lt8t->next_node->pt  =  assign_point_value8( f rom_point) ; 
return  (list); 
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/* 


PLUS  TANGENT 


- qQq - 

This  functions  finds  and  returns  the  node  which  with  the  inputted  point 
forms  the  plus  common  tangent.  It  calls  the  the  order  function.  Sign  is 
a  macro  and  is  defined  in  3d_tan.h  and  returns  an  integer  corresponding 
to  the  value  returr-'^d  by  order.  The  function  returns  the  point  found  to 
the  calling  function*/ 


point  plus_tangent(start,  pi) 

obstacle_plane  *start; 
point  pi; 

{ 

obstacle_plane  *next  =  start->ccw,  *prev  =  start->cw,  ‘current  =  start; 

int  count; 

point  pt; 

double  dirl,  dir2; 


for  (count  ■=  1;  count  <=  start- >number_nodes;  count++)  { 

if  (current->pt . X  -=■  pl.x  &&  current->pt.y  -=  pl.y)  { 
current  =  current->ccw; 
break; 

1 


if  (sign(order (pi,  current,  next))  ==  1)  { 
current  =  next; 
next  “  next->ccw; 

1 

else  ( 

if  (sign(order(pl,  current,  prev))  ==  1)  { 
current  =  prev; 
prev  =  prev->cw; 

) 

else 

break; 

) 

1 

if (atan2( pi . y  -  current->pt . y ,  pl.x  -  current->pt.x)  == 

atan2(pl.y  -  current->ccw->pt.y,  pl.x  -  current->ccw->pt.x) ) { 
if (distance(pl,  current->pt)  <  distance(pl,  current- >ccw->pt ) ) 
return(current->pt) ; 
else 

return(current->ccw->pt) ; 

) 

if (atan2(pl.y  -  current->pt.y,  pl.x  -  current->pt.x)  == 

atan2(pl.y  -  current->cw->pt.y,  pl.x  -  current->cw->pt .x) ) { 
if (dlstance(pl,  current->pt)  <  distance(pl,  current->cw->pt) ) 
return(current->pt) ; 
else 

return(current->cw->pt) ; 

) 

return  (current->pt) ; 
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MINUS  TANGENT 


/* 


- oOo - 

finds  and  returns  the  node  in  the  polygon  which,  with  the  inputted  point 
forms  the  minus  common  tangent.  It  calls  the  the  order  function.  Sign 
is  a  macro  and  Is  defined  in  3d_tan.h  and  returns  an  integer  corresponding 
to  the  value  returned  by  order.  The  function  returns  the  point  found  to 
the  calling  function*/ 

point  minus_tangent(start,  pi) 

obstacle_plane  ‘start; 
point  pi; 

{ 

obstacle_plane  *next  =  start->cw,  *prev  -=  start->ccw,  ‘current  =  start; 
int  count; 

point  pt; 

for  (count  -  1;  count  <-  start- >number_nodes;  count++)  { 

if  { current ->pt.x  --  pl.x  6&  current->pt.y  ==  pl.y)  I 
current  «  current- >cw; 
break; 

1 

if  (sign (order (pi,  current,  next))  ==  -l)  { 
prev  -  current; 
current  «  next; 
next  =  next->cw; 

1 

else  ( 

if  (sign(order (pi,  current,  prev))  ==  -l)  { 
next  =  current; 
current  «  prev; 
prev  »  prev->ccw; 

) 

else 

break; 


if (atan2(pl.y  -  current->pt.y,  pl.x  -  current->pt .x)  == 

atan2(pl.y  -  current->ccw->pt . y ,  pl.x  -  current->ccw->pt.x) ) ( 
if (dlstance(pl ,  current->pt)  <  dlstance(pl,  current->ccw->pt) ) 
return(current->pt) ; 
else 

return(current->ccw->pt) ; 

) 

if (atan2(pl .y  -  current->pt.y,  pl.x  -  current->pt.x)  == 

atan2(pl.y  -  current->cw->pt.y,  pl.x  -  current->cw->pt .x) ) ( 
if (distance(pl,  current->pt)  <  distance(pl,  current->cw->pt) ) 
return ( current ->pt) ; 
else 

return(current->cw->pt) ; 

1 

return  (current->pt) ; 
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APPENDIX  J 


This  appendix  contains  the  source  code  of  vertical.c.  This  code  corresponds  to 
those  functions  used  to  find  the  vertical  path  over  a  polyhedron. 

#include  "3d_tan.h" 

#lnclude  "plot.h" 

idefine  LOOP 

/* . . . 

FIND  VERTICAL  NODES 

. OOO . . . 

This  funi-tlon  finds  the  appropriate  vertical  points  formed  by  the  inter¬ 
section  oZ  the  vertical  plane  and  each  polyhedron.  The  function  cycles 
through  the  polyhedron  list  checking  each  polyhedron  and  finding  all 
intersection  points  for  that  polyhedron.  It  then  determines  the  two 
highest  points  and  checks  checks  if  the  intersection  points  are  between 
the  expanded  point  and  the  goal.  If  the  points  are,  the  function  calls 
new_vertlcal_new_list,  which  places  one  of  the  two  or  both  of  the  points 
into  a  vertical  list  (link  list).  The  function  then  checks  the  next 
polyhedron,  finding  the  appropriate  Intersection  points  and  calls  the 
new_vertical_new_list  which  replaces  the  points  in  the  vertical  list  if 
they  satisfy  the  requlrments  of  the  heuristics  in  that  function.  It 
recleves  the  vertical  list  from  new_vertical_new_list  and  passes  it  to 
the  calling  function  upon  completion.*/ 

vertlcal_path  *f ind_vertical_nodes(expand_pt,  end_pt) 

point  expandpt,  end_pt;  /»  expand_pt  is  the  point  from 

*  which  the  vertical  list  is 

*  formed,  end_point  is  the 

*  ending  point  that  is  used 

*  to  form  the  vertical  list  */ 

( 

point  ptl,  pt2,  temp; 

obatacle_list  *current_polyhedron; 
polyhedronobstacle  ‘face; 
polyhedron_obstacle_plane  *current_node; 

int  count,  boolean  =  FALSE,  two_points  *  FALSE; 

vertlcal_path  ♦vertical_list  ”  NULL; 

vertical_n_perpendlcular_plane(expand_pt,  end_pt) ; 

for  {current_polyhedron  -  polyhedron_li8t; 
current_polyhedron  I-  NULL; 

current_polyhedron  -  current_polyhedron->next_polygon, 
two_polnts  -  FALSE,  boolean  ”  FALSE)  { 

for  (face  «  current_polyhedron->obstacle; 

face  !“  NULL;  face  =  face->next_face)  { 

for  (current_node  -  face->face_nodes,  count  -  1; 
count  <-  face->face_nodes->number_nodes; 
current_node  -  current_node->ccw,  count++)  { 
if  (two_points  "  FALSE)  { 
if  (boolean  — =  FALSE)  ( 

ptl  ”  intersection_point(current_node->pt, 

current_node->ccw->pt,  vertical); 
if  (ptl.X  !=  NOINTERSECTION)  ( 
boolean  -  TRUE; 
continue; 
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) 

1 

else  { 

pt2  -  lntersection_point(current_node->pt, 

current_node->ccw->pt,  vertical) ; 
if  (pt2.x  I-  NO_INTERSECTION)  ( 
two_poirts  -=  TRUE; 
if  (ptl.z  >  pt2.z)  { 
temp  “  swap(ptl); 
ptl  -  swap(pt2); 
pt2  8wap(temp); 

1 

1 

} 

1 

else  ( 

temp  -  intersection_polnt(current_node->pt, 

current_node->ccw->pt,  vertical) ; 
if  (temp.x  !=  NO_INTERSECTION)  { 

if  (temp.x  “  ptl.x  &&  temp.y  -=  ptl.y  &&  temp.z  ==  ptl.z)  [ 
if  (temp.x  ==  pt2.x  &&  temp.y  -=  pt2.y  6&  temp.z  ==  pt2.z)  { 
continue; 

) 

) 

if  (temp.z  <  ptl.z)  ( 
pt2  -  swap (ptl); 
ptl  =  swap (temp); 

J 

else  ( 

if  (temp.z  <  pt2.z) 
pt2  =  swap (temp); 

1 

1 

) 

) 

1 

if  (ptl.x  !-  NO_INTERSECTION  66  boundry_check (boundry , ptl )  iS 
boundry_check(boundry,  pt2)  66 

(ptl.x  I-  end_pt.x  | |  ptl.y  t-  end_pt.y)  66  (ptl.x  l=  expand_pt.x  | | 
ptl.y  1=  expand_pt.y)  66 

(pt2.x  I-  expand_pt.x  ||  pt2.y  1=  expand_pt . y ) )  {  /*the  or  checks  are 
to  ensure  the  expand  or  end  point  are  not  included  as 
in  the  case  of  a  face  edge*/ 

if  (vertical_node_direction_heurlstic(end_pt,  expand_pt,  ptl)  66 
vertlcal_node_direction_heuristic(end_pt,  expand_pt,  pt2)) 

vertical_l ist  -  vertical_node_list_f rom_pt ( vert ical_l is t , ptl , pt2 , 

expand_pt,  current_polyhedron->polyhedron_number ) ; 


1 

) 

return  (vertical  llst) ; 


point  swap(pt) 

point  pt; 

{ 

point  ptl; 

ptl.x  -  pt.x; 
ptl.y  -  pt.y; 
ptl.z  “  pt.z; 
return  (ptl); 

) 
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/* . - . 

NEW  VERTICAL  NODE  LIST  FROM  POINT 


This  function  uses  the  two  vertical  points  passed  to  it  from  find_verti- 
cal_nodes,  which  represent  the  vertical  nodes  from  a  polyhedron,  and 
places  them  in  euclcuian  distance  order  in  a  vertical_list .  It  returns 
to  the  calling  function  this  vertical_list. 

•/ 

vertical_path  *vertical_node_list_from_pt{vertical_list,ptl,pt2,  pt,  poly_num) 
vertical_path  *vertical_list; 
point  ptl,  pt2,  pt; 

int  poly_num; 

{ 

double  ptl_distance,  pt2_distance,  pt_dlst; 

a_path  ‘current,  ‘previous,  ‘temp; 

ptl_di8tance  ■=  distance(ptl,  pt); 
pt2_di8tance  “  distance(pt2,  pt); 

NEW_NODES(temp) ; 
temp->next_node  =  NULL; 

if (ptl_distance  ==  pt2_distance) { 

temp->pt  =  assign_point_values (ptl ) ; 
temp->polygon_number  -  poly_num; 

1 

if (ptl_dlstance  I=  pt2_distance) ( 

NEW_NODES(temp->next_node) ; 
temp->next_node->next_node  =  NULL; 
if (ptl_dlstance  >  pt2_distance) ( 

temp->pt  -  assign_point_values(ptl) ; 
temp->next_node->pt  =  assign_point_values(pt2) ; 

1 

else{ 

temp->pt  “  assign_point_values(pt2); 
temp->next_node->pt  •  asslgn_point_values(ptl ) ; 

) 

temp->polygon_number  =  poly_num; 
temp->next_node->polygon_number  =  poly_num; 

) 

if (vertical_list  =-  NULL)( 

NEW_VERTICAL(vertical_list) ; 
vertical_list->next_node  =  temp; 

) 

el8e( 

for(current  =  vertical_list->next_node,  previous  =  NULL; 
current  I-  NULL  | |  temp  »=  NULL; 
previous  =  current- >next_node, 
current  =  current- >next_node->next_node) ( 

pt_di8t  -  dl8tance(pt,  current->pt) ; 

if (ptl_dlstance  >  pt_dlst  ||  pt2_distance  >  pt_dlst)( 
temp->next_node->next_node  =  current; 
if (previous  --  NULL)( 

vertical_list->next_node  -=  temp; 
break; 

) 

else 

previous- >next_node  =  temp; 

) 

if (current->next_node  •==  NULL)(  /‘considers  only  one  node  in  list 

and  temp  nodes  come  after  lt‘/ 
current->next_node  -  temp; 
break; 

) 

1 

If  (temp  NULL  &&  previous  !=  NULL) 
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prevlous->next_node  =  temp; 

1 

vertical_li8t->next_node->next_after_node  -  NULL; 
return (vertical_list) ; 


/* 


INSERT  VERTICAL  LIST 

. OOO . . . . 

This  function  takes  the  vertical  list  and  Inserts  the  list  as  a  valid 
path  In  the  8tart_paths  list. 

•/ 

paths  *insert_verticle_list(start_paths,  current_path,  vertical_list,  tan4vert, 

end_pt,  polyqon4vert) 


paths  *start_paths,  *current_path; 

vertlcal_path  *vertical_list; 

Int  tan4vert; 

point  end_pt; 

paths  *temp; 
a_path  ‘current; 
double  cost  -  0; 


NEW_PATH{temp); 

temp->last_node  -  vertical_llst->next_node; 
current  -  temp->last_node; 
current ->next_after_node  =  NULL; 
temp->last_node->next_after_node  •=  NULL; 
free<vertlcal_llst) ; 


/‘connect  the  double  link  list  and  calculate  the  distance  from  the 
first  node  to  the  last  node‘/ 
for(  ;  LOOP  ;  current  -  current -> next_node ) ( 
if (current- >next_node  1-  NULL) 1 

current->next_node->next_after_node  •=  current; 

cost  +-  distance(current->pt,  current->next_node->pt) ; 

) 

else 

break; 

1 

temp->polygon4vert  -  polygon4vert; 
temp->polygon4vert_PTR  ”  NULL; 
temp->cost  “  cost; 

temp->estimated_cost  -  distance(temp->last_node->pt,  goal); 
temp->total_cost  -  cost  +  temp->estimated_cost; 
temp->type_tangent  -  VERTICAL; 

temp->ray_direction  =  atan2( (temp->last_node->pt.y  - 

temp->last_node->next_node->pt . y ) / 
(temp->last_node->pt.x  - 
temp->last_node->next_node->pt.x) ) ; 

temp->plane_number  *  0; 
temp->when_path_found  =  paths_found++; 
temp->able2expand  -  TRUE; 

temp->polygon4vert_PTR  -  lnput_data4polyhed4vert( temp->polygon4vert_PTR, 

end_pt,  tan4vert); 


temp->next_path  ”  current_path->next_path; 
lf(start_paths  --  NULL) 

8tart_paths  -  temp; 
else( 

temp->next_path  -  current_path->next_path; 
current_path->next_path  *  temp; 
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ttifdef  DOS 

if  (plot2screen) ( 

for(current  =  tenip->last_node; 
current->next_node  !=  NULL; 
current  ”  current ->next_node) 

draw_line_segment(current->next_node->pt,  current->pt,  YELLOW, 

REGULAR) ; 

) 

#endlf 

return  (start_paths) ; 

) 


/* . . . 

VERTICAL  NODE  DIRECTION  HEURISTIC 

. ooo . . 

This  function  determines  if  a  point  lies  in  the  same  xy  direction  from  a 
start  point  as  the  direction  of  the  line  from  the  start_pt  to  the  goal. 
The  8tart_pt  is  not  the  same  as  the  global  start_point  but  just  a  local 
variable  and  could  any  valid  point  in  the  valid  possible  path  list 
*/ 

int  vertical_node_direction_heuristic(goal,  expand_pt,  pt) 

point  goal,  expand_pt,  pt; 

[ 

double  X,  y,  a; 

if(goal.x  "  expand_pt.x  ||  pt.x  ■=*  expand_pt.x) 
expand_pt.x  +■=  .0000001; 
if(goai.x  «-  pt.x) 

goal  -  lncrease_goal_x(goal,  expand_pt); 

X  -  atan2(goal.y  -  expand_pt.y,  goal.x  -  expand_pt.x) ; 
y  -  atan2<goal.y  -  pt.y,  goal.x  -  pt.x); 
a  -  X  -  y; 

if  <ABS(a)  <-  QPI) 
return  (1); 
else 

return  (0); 


/* . 

INCREASE  GOAL  X 

. OOO . . . 

This  function  is  called  if  its  determined  that  the  increase  and  pt2  points 
have  the  same  value  for  the  x  coordinate.  if  so  then  it  adds  of  subtracts 
.000001  to  the  x  component  goal  so  that  the  new  distance  from  the  two 
points  is  greater  than  the  original  distance.  If  not  done,  atan2  is 
undetermined  since  x  -  x  *=  0 
V 

point  increase_goal_x ( increase,  pt2) 
point  increase,  pt2; 

( 

double  orginal_dlstance; 

orginal_distance  -  distance( increase,  pt2); 
increase. x  +»  .0000001; 

if (di8tance(increase,  pt2)  <  orglnal_distance) 
increase. X  --  .0000002; 

return{ increase) ; 
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/* . . . 

UP  AND  OVER  PATHS 

. . . OOO . . . 

This  function  calls  two  other  functions.  The  first  finds  the  verti¬ 
cal  nodes  of  a  possible  path  and  places  them  in  a  list  labled  verti- 
cal_ll8t.  The  second  function  takes  the  vertical_list,  places  it 
in  the  possible  paths  list  and  updates  the  header  list  of  the  path. 

*/ 

paths  *up_and_over(8tart_paths,  current_path,  exp_pt,  end_pt,  beginpt,  mode, 
polygon4vert) 

paths  *start_paths,  *current_path; 

point  exp_pt,  end_pt,  beginpt; 

int  mode,  polygonivert; 

( 

vertical_path  *vertical_list  “  NULL; 
a_path  *current; 

vertlcal_list  =  f ind_vertical_nodes(exp_pt,  end_pt); 
if  (vertlcal_ll8t  t=  NULL){ 

lf(beginpt.x  --  exp_pt.x  &&  beginpt. y  “=  exp_pt.y  && 
beginpt. z  =-  exp_pt.z){ 
for(current  =  vertlcal_list->next_node; 
current ->next_node  1=  NULL; 
current  =  cur rent -> next_node ) ; 

NEW_NODES(current->next_node) ; 

current->next_node->next_node  =  NULL; 
current->next_node->pt  •=  asslgn_point_values (exppt) ; 

1 

vertlcal_list  =  trlm_not_needed_nodes(vertlcal_list) ; 
start_paths  =  insert_verticle_list(start_paths,  current_path, 

vertical_llst,  mode,  end_pt, 
polygon4vert) ; 

1 

if (VERTICAL  «=  mode){) 
return ( startpaths ) ; 

} 

/* . 

CONTINUE  VERTICAL  PATH 

. OOO . 

This  function  finds  the  one  node/tangent  which  can  extend  the  verti¬ 
cal  path  by  one  node  on  the  perpendicular  plane  formed  by  the  verti¬ 
cal  node  and  the  goal.  There  is  only  one  node  to  consider  since  it 
falls  on  the  line  segment  of  the  polyhedron  used  to  find  the  verti- 
node 
*/ 

paths  *continue_vertlcal_path(8hortest,  8tart2d) 
path8  *shortest; 
polygonllst  *start2d; 

( 

polygon_ll8t  ^polygon  =  8tart2d; 

point  ptl,  pt2; 

a_path  *temp; 

int  tan, found  -  FALSE; 

double  ray_directlon,  difference; 

for  (;  polygon  I-  NULL;  polygon  =  polygon->next_polygon) ( 
if (polygon->plane_number  1=  8horte8t->plane_number ) 
continue; 

if (polygon->polyhedron_number  1“  shortest- >polygon4 vert) 
continue; 
else 
break; 

) 
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if(l  ==  shortest->polygon4vert_PTR->type_tangent) 

ptl  =  plus_tangent(polygon->polygon_start,  shortest->last_node->pt) ; 
else 

ptl  -  jninus_tangent(polygon->polygon_8tart,  shortest->last_node->pt ) ; 

ray_direction  =  atan2(ptl.y  -  shortest->last_node->pt . y, 

ptl.x  -  shortest->last_node->pt.x) ; 

/♦used  le-3  instead  of  0  because  of  the  precision  of  DOS  machine.  No 
points  used  evaluated  true  and  the  point  which  should  have  evalueated 
true,  had  a  difference  of  le-10*/ 


difference  =  shortest- >ray_direction  -  ray_direction; 

/♦find  the  up_around  and  up  paths  before  continuing  on*/ 

if (ABS(difference)  <  le-3){ 

NEW_NODES(temp) ; 

temp->pt  “  asslgn_point_values(ptl) ; 
temp->polygon_number  =  polygon->poiygon_number; 
temp->next_node  =  shortest->last_node; 
shortest->last_node  =  temp; 

if {8hortest->polygon4vert_PTR->type_tangent)  tan  =  PLUS; 

else  tan  =  MINUS; 

shortest  •=  update  (shortest,  tan); 

found  -  TRUE; 

] 

else{ 

shortest  ”  recalculate_vertlcal_path(ptl,  shortest, 

( shortest->polygon4vert_PTR->t /pe_tangent 

MINUS) ) ; 

shortest->last_node->polygon_number  =  polygon->polygon_number; 
found  =  TRUE; 


?PLUS 


if  (I  found) 

shortest->able2expand  =  NO; 

#ifdef  DOS 
else( 

if  (plot2screen ) 

draw_l ine_segment ( shortest - > last_node- >next_node- >pt , 
shortest->last_node->pt,  YELLOW,  REGULAR); 


J 

#endif 

return { shortest ) ; 

1 


/* . . 

UP  AND  OVER  FROM  GOAL 

. OOO . 

This  function  finds  all  the  vertical  partial  paths  from  the  goal 
extending  to  the  start.  This  working  backwards  from  the  goal  allows 
me  to  find  the  valid  paths  which  go  around  one  or  more  obstacles  and 
over  the  rest  of  them. 

*/ 

paths  *up_and_over_from_goal( paths  *start_paths,  paths  *current_path, 
point  goal,  polygon_li8t  *8tart2d) 

/*  paths  *8tart_paths ,  *current_path; 
point  goal; 

polygon_li8t  *8tart2d; 

*/ 

1 

polygon_llst  *current_polygon; 
point  ptl,  pt2; 
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int  poly4pt; 

for  (current_polygon  =  start_2d; 
current_polygon  1=  NULL; 

current_polygon  =  current_polygon->next_polygon )  { 

ptl  -  plus_tangent{current_polygon->pclygon_start,  goal); 
pt2  -  ininus_tangent(current_polygon->polygon_start,  goal); 

if  (cross_polygon (atart_2d,  goal,  ptl)  && 
direction_heuristlc_froni_start(goal,  ptl)  && 
boundry_check(boundry,  ptl))  { 

Btart_paths  =  up_and_over(8tart_paths,  current_path,  goal, 

ptl,  goal,  PLUS,  current_polygon->polyhedron_number ) ; 
if  (current_path  =-  NULL)  current_path  =  start_paths; 
else  current_path  =  current_path->next_path; 
current_path->?orward  *=  NO; 

1 

if  (cro8s_polygon(start_2d,  goal,  pt2) 
direction_heuriatic_from_8tart(goal,  pt2) 
boundry_check(boundry ,  pt2))  { 

8tart_paths  =  up_and_over (start_paths,  current_path,  goal, 
pt2,  goal,  MINUS); 

if  (current_path  -=  NULL)  current_path  •=  start_paths; 
else  current_path  =  current_path->next_path; 
current_path->forward  “  NO; 

current_path->polygon4vert  =  current  polygon->polyhedron  number; 

) 

1 

return (8tart_paths) ; 


paths  *recalculate_vertlcal_path(pt,  path,  tan) 
point  pt; 
paths  *path; 
int  tan; 

( 

verticalpath  »vertical; 
a_path  •node  -  path->last_node, 

•current,  •previous;  ~ 
double  cost  •=  0; 
int  count,  number_nodes; 

/•find  the  last  node  (or  beginning  pt  of  path)^/ 
for{  ; node->next_node  1“  NULL;  node  =  node->next_node) ; 

/•disconnect  the  last  node  from  the  path^/ 
node->next_after_node->next_node  =  NULL; 
node->next_af ter_node  *  NULL; 

/•delete  the  path  nodes  of  the  shortest_path 
while  counting  number  of  nodes^/ 

for(current  -  path->la8t_node,  previous  «  current,  number_nodes  =  0; 
current  I*-  NULL; 

previous  -  current,  current-  current->next_node,  number_nodes++) 
free ( previous ) ; 

f ree( previous ) ; 

/•find  the  new  vertical  list*/ 

vertical  -  f ind_vertical_nodes ( node->pt,  pt); 

/•finds  the  end  of  the  vertical  list  then  working  backwards  up  the 
list,  places  the  same  number  of  nodes  in  the  list^/ 
for{current  -  vertlcal->next_node;  current->next_node  1=  NULL; 
current  -  current->next_node) 
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current ->next_node->next_after_node  =  current; 

for (count  -  1;  count  <  number  nodes ; 

count++,  current=  current~>next_after_node) ; 
vertlcal->next_node  -  current; 

/•frees  up  the  unused  portion  of  the  list*/ 
for(current  =  current->next_after_node;  current  1=  NULL; 
current  -  current->next_after_node) 

free (current) ; 

path->la8t_node  =  vertical->next_node; 
free( vertical ) ; 

path  -  insert_pt_into_list(path,  pt); 
path->last_node->next_after_node  =  NULL; 

for(current  -  path->last_node; 
current- >next_node  1=  NULL; 
current  »  current->next_node) ( 
current->next_node->next_after_node  =  current; 
cost  +-  di8tance(current->pt,  current->next_node->pt) ; 

current- >next_node  -=  node; 
node->next_af ter_node  =  current; 
cost  +“  distance(current->pt,  node->pt); 
if ( path->forward  ==  YES) 

path->estimated_cost  -=  distance(node->pt,  goal); 
else 

path->estlmated_cost  -  distance(node->pt,  start_point) ; 

path->total_cost  =  cost  +  path->estimated  cost; 
path->cost  “  cost;  ~ 

path->ray_direction  =  atan2(path->last_node->pt.y  - 

path->last_node->next_node->pt . y , 
path->last_node->pt.x  - 
path->last_node->next_node->pt . x) ; 

path->type_tangent  =  tan; 
return ( path ) ; 

) 

paths  •in8ert_pt_lnto_list(path,  pt) 
paths  ‘path; 
point  pt; 

{ 

apath  »temp  -  NEWNODES ( temp ) ; 
temp->pt  -  assign_point_values(pt) ; 
temp->next_after_node  -  NULL; 
temp->next_node  -  path->last_node; 
path->last_node  =  temp; 

return (path) ; 

) 

polyhed4  *input_data4polyhed4vert(ptr,  end_pt,  mode) 
polyhed4  *ptr; 
point  end_pt; 
int  mode; 

{ 

if(NULL  ~  ptr) 

ptr  -  (polyhed4  »)  malloc(sizeof (polyhed4 ) ) ; 

ptr->x  »  endpt.x; 
ptr->y  -  endpt.y; 
ptr->type_tangent  »  mode; 

return (ptr) ; 

1 
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vertical_path  •triin_not_needed_nodes ( list ) 
vertical_path  ‘list; 
i 

a_path  *nodel,  *node2,  *node3,  *node4; 

for(nodel  -  list->next_node; 

NULL  I-  nodel; 

nodel  ”  nodel->next_node) { 

if(NULL  •“  nodel->next_node->next_node)  break; 

for(node2  nodel->next_node->next_node; 

NULL  I-  node2; 

node2  -  node2->next_node) { 

if ( lnter8ect_polyhedron{nodel->pt,  node2->pt))  continue; 
else( 

for (nodes  -  nodel->next_node,  node4  =  node3; 
node2  I-  nodeS; 

node4  -  nodes,  nodeS  -  node3->next_node, free(node4 ) ) 

nodel->next_node  -  node2; 
node2->next_after_node  =  nodel; 

} 

) 

1 

return(ll8t); 

) 
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APPENDIX  K 


This  appendix  contains  the  source  code  associated  with  vlslblle.c. 

# Include  ■3d_tan.h" 

# Include  "plot.h" 

#deflne  LOOPFOREVER  1 


- - - qOo - 

VISIBILITY 

- - - qOo - 

This  function  determines  whether  or  no  the  last  point  In  the  partial  path  Is 
visible  to  the  goal  or  not.  The  function  considers  whether  a  point  on  the 
backside  of  a  polygon  Is  visible  to  the  goal  by  first  finding  the  tangents  to 
the  goal  with  the  polygon  and  traces  the  path  around  to  the  expansion  point. 

A  point  Is  valid  In  this  case  If  no  obstacles  lie  between  the  goal  and  the 
tangent  point 
*/ 

a_path  ‘visibility (expand_polnt,  goal_polnt,  start,  two_d_llst) 
point  expand_polnt,  goal_polnt,  start; 
polygon_llst  *two_d_llst; 

{ 

polygon_llst  ‘polygon; 
point  f rom_polygon_pt,  goal; 
a_path  ‘temp,  ‘tempi,  ‘goal_llst; 
obstacle_plane  ‘node; 
int  tangent  »  1; 

goal  -  determlne_end_polnt( start,  goal_polnt); 
lf{ I ii!ter8ect_polyhedron(expand_polnt,  goal)) { 

NEW_NODES ( goa 1_1 Is  t ) ; 
goal_llst->next_node  =  NULL; 
goal_llst->pt  «  asslgn_polnt_values (goal ) ; 
return(goal_llst) ; 

) 

If (shortest_path->type_tangent  ==  VERTICAL) 
return(NULL) ; 
else( 

/‘find  polygon  which  last  node  of  shortest  path  lies  on*/ 
for(polygon  -  twodllst; 

polygon- >polygon_number  I*  shortest_path->last_node->polygon_number  | | 
polygon- >plane_number  I-  8hortest_path->plane_number ; 
polygon  -  polygon ->next_polygon); 

whlle{tangent  <=  2)[ 
lf(l  “  tangent) 

/‘finds  the  plus  tangent  from  the  goal  to  the  polygon  last  node  of 
shortest  path  lies  on*/ 

from_polygon_pt  -  plus_tangent(polygon->polygon_start,  goal); 
else 

/‘check  the  minus  tangent*/ 

from_polygon_pt  -  mlnus_tangent(polygon->polygon_start,  goal); 

/‘checks  for  Intersection,  If  there  Is  return  a  zero*/ 

If ( lnter8ect_polyhedron( from_polygon_pt,  goal ) ) 
return (NULL) ; 

else(  /‘two  points  are  visible*/ 

If (temp  “  NULL)( 

NEW_NODES(temp) ; 
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temp- >next_ node  -  NULL; 
temp->next_after_node  =  NULL; 

) 

temp->pt  =  assign_point_values( from_polygon_pt) ; 
tempi  “  temp; 

/•finds  the  node  on  the  polygon  which  the  from_polygon_pt  lies  on*/ 
for  (node  -  polygon- >polygon_start- 
node->pt.x  from_polygon_pt.x 
node->pt.y  1=  from_polygon_pt.y 
node->pt.z  1“  frompolygonpt.z; 
node  “  node->ccw); 

while  (LOOPFOREVER)  { 
if(l  “  tangent)! 

/•checks  to  see  if  the  next  counter  clockwise  node  is  the 
expansion  node*/ 

if (node->ccw->pt.x  ==  shortest_path->last_node->pt . x  && 
node->ccw->pt .y  =■=  shortest_path->last_node->pt.y 
node->ccw->pt.z  ==  shortest_path->last_node->pt.z) 
break; 
else( 

if (templ->next_node  NULL){ 

NEWNODES ( tempi -> next_node ) ; 

tempi ->ncxt_node->next_after_node  =  tempi; 

templ->next_node->next_node  =  NULL; 

1 

tempi  =  tempi ->next_node; 

templ->pt  -  assign_polnt_values(node->ccw->pt) ; 

) 

node  »  node->ccw; 

1 

el8e(  /*tangent  =*  2*/ 

/•checks  to  see  if  the  next  counter  clockwise  node  is  the 
expansion  node*/ 

if (node->cw->pt.x  shortest_path->la8t_node->pt.x  && 
node->cw->pt.y  *=  shortest_path->last_node->pt.y 
node->cw->pt.z  •-  shortest_path->last_node->pt . z) { 
if (tempi ->next_node  I-  NULL){ 
for (tempi  -  tempi ->next_node, 

templ->next_after_node->next_node  -  NULL, 
templ->next_after_node  =  NULL, 
tempi- >next_node; 
tempi- >next_node  I=  NULL; 
tempi  “  tempi ->next_node) 
free( tempi ->next_after_node) ; 
if (tempi ->next_after_node  1=  NULL) 
free(templ->next_after_node) ; 
free( tempi ) ; 

for(teropl  “  temp;  temp->next_node  !-  NULL;  temp  -=  temp->next_node) 
break; 

1 

else! 

if (templ->next_node  NULL){ 

NEW_NODES( tempi ->next_node) ; 

tempi- >next_node->next_after_node  -  tempi; 

templ->next_node->next_node  *=  NULL; 

1 

tempi  =  tempi ->next_node; 

templ->pt  -  assign  point_values(node->cw->pt) ; 

) 

node  -  node->cw; 

1 

) 

if  (direction_heuristic(shortest_path,  templ->pt)){ 
goallist  -  temp; 
return(goal_li8t) ; 
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1 

1 

tangent  +=  1; 

1 

) 

for(teinpl  -  temp->next_node;  tempi ->next_node  1=  NULL;  tempi  =  tempi ->next_node) 
free(templ->next_after_node) ; 
free(temp->next_after_node) ; 
free ( temp ) ; 
return (NULL) ; 


point  determlne_end_point( start,  goal) 
point  start,  goal; 

{ 

if (8hortest_path->forward)  return(goal) ; 
else  return ( start) ; 

) 
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APPENDIX  L 


This  appendix  contains  the  two  header  files  (3d_tan.h  and  plot.h). 

# Include  <math.h> 

# Include  <stdio.h> 


/* 


3D  TANGENT  HEADER  FILE  (3D_TAN.H) 

- qQq - 


*/ 


#deflne  READONLY  "r* 

#define  WRITEONLY  "w" 

#deflne  TRUE  1 

#deflne  FALSE  0 

Hdeflne  NO  0 

#deflne  YES  1 

#define  NO_INTERSECTION  999999.9990 

#define  sqr{x)  (x  *  x) 

#define  F{X,Y,Z)  ( (pl_eq.x»temp_pt.x)  +  (pl_eq.z*temp_pt.z) )/pl_eq.y 

#deflne  VERTICAL  2 

#deflne  NONE  2 

#deflne  PLUS  1 

#deflne  MINUS  0 

(♦define  GOAL_PT  1 

(♦define  NOT_GOAL_PT  0 

#deflne  signja)  (((a)  <  0)  ?  -1  :  (a)  >  0  ?  1  :  0) 

(♦define  ABS(x)  (<(x)  <  0)  ?  -x  :  (x)  >  0  ?  x  :  0) 

#deflne  TOTAL_COST  current_path->cost  +  current_path->estiiiiated  cost 

# define  FOREVER  1 

(♦define  QPI  0.785398230785398 

#deflne  HPI  1.5707963257949 

(♦define  PI  3.141593123141593 

idefine  DPI  6.283186246283186 

(♦define  NEW_OBSTACLE(x)  ((x  -  (obstacle  *)  malloc 

(slzeof  (obstacle)  )  )”NULL?exit  ( 1) :  1 ) 

(♦define  NEW_PLANE(x)  ( (x  -  (obstacle  plane  *)  malloc 

(slzeof  (obstacle_plane  ) ) )==NULL?exit( 1 ) : 1 ) 

#deflne  NEW_NODE(x)  ( (x  =  (obstaclelist  *)  malloc 

(slzeof (obstacle_li8t)) )  -»  NULL?exit(l) : 1) 

#deflne  NEW_POLYHEDRON(x)  ((x  -  (polyhedron_obstacie  •)  malloc 

(slzeof (polyhedron_obstacle) ) )  «=  NULL?exit( 1 ) . 1 ) 
(♦define  NEW_POLY_PLANE(x)  ((x  -  (polyhedron_obstacle_plane  •)  malloc 

(slzeof (polyhedron_obstacle_plane) ) )  ==•  NULL?exit(l) :1) 
#deflne  NEW_VERTICAL(x)  ( (x  '  (vertlcal_path  *)  malloc 

(slzeof (vertlcal_path)))  =«  NULL?exit(l) : 1) 

#deflne  NEW_GOAL(x)  ( (x  “  (goals  *)  malloc 

(slzeof (goals)) )  —  NOLL?exit(l) :  1) 

#define  NEW_PATH(x)  ((x  -  (paths  *)  malloc 

(slzeof (paths) ) )  — ■  NULL?exit(l) :  1) 

♦define  NEW_NODES(x)  ((x  “  (a_path  •)  malloc 

(sizeof (a_path) ))  --  NULL?exit(l) : 1) 

(♦define  NEW_P0LYG0N(x)  ((x  -  (polygon  llst  *)  malloc 

(sizeof (polygon_li8t) ) )  --  NULL?exit( 1 ) : 1 ) 

♦define  NEW_ACTlVE_NODE{x)  ((x  «  (active_nodes  *)  malloc 

(sizeof (active_nodes) ) )  --  NULL?exit(l) : 1) 

♦define  POINT_VALOES  Sx_coord,  4y_coord,  6z_coord 
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Klfdef  MAIN 

#  define  EXTERN 

#  define  INIT(Value)  =  Value 
#else 

#  define  EXTERN  extern 

#  define  lNIT{Value) 

#endlf 


typedef  struct  polyhedronllst  { 
struct  polyhedron  ‘obstacle; 

Int  number_faces; 

Int  polyhedron_number ; 

struct  polyhedron_llst  *next_polygon; 
)  obstacle_llst; 


EXTERN  ob8tacle_llst  ‘poly hedron_l 1st  INIT{NULL}; 

EXTERN  obstacle_llst  ‘lntersected_polyhedron  INIT(NULL); 


typedef  struct  polyhedron  ( 

struct  polyhedron_node  ‘face_nodes; 

Int  number_nodes; 

struct  polyhedron  ‘next_face; 

1  polyhedron_obstacle,  polyhedron_face; 


typedef  struct 
double 
double 
double 
double 
double 
double 


posture  ( 
x; 

y: 

z; 

phi; 

t; 

psl; 

POSTURE; 


typedef  struct 
double 
double 
double 

1 


x_y_z  ( 

x; 

y; 

z; 

point; 


typedef  struct  polyhedron_node  { 
struct  x_y_z  pt; 

Int  number_nodes ; 

struct  polyhedron_node  *cw; 
struct  polyhedron_node  ‘ccw; 

1  polyhedron_obstacle_plane,  polyhedron_nodes,  obstacle_plane 


EXTERN  point 


start_polnt,  goal.  Intersection; 


typedef  struct  plane_equatlon  { 
double  x; 

double  y; 

double  z ; 

double  d; 

)  equatlon; 


EXTERN  equation  vertical,  perpendicular; 

typedef  struct  twodpolygonllst  ( 
obstacleplane  ‘polygonstart; 
struct  two_d_polygon_llst  *next_polygon; 


Int 

plane_number ; 

int 

polygon_nuTnber ; 

int 

polyhedron_number 

) 

polygon_llst; 

EXTERN  polygonllst  *start_2d  INIT(NULL); 
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/•typedef  struct  polygonnode  { 
struct  yi_y_z  pt; 
int  nuniber_nodes  ; 

struct  polygon_node  *ccw; 
struct  polygon_node  *cw; 

1  obstacle_plane; */ 

typedef  struct  list_of_active_nodes  { 
point  pt; 

double  distance; 

struct  list_of_active_nodes  *next_active_node; 
Int  when_path_found; 

)  actlve_nodes; 


EXTERN  active  nodes  *head  actlve_nodes  INIT(NULL); 


typedef  struct  list_of_paths  { 

struct  llst_of_paths  *next_path; 
struct  possible_paths  *last_node; 


double 
double 
double 
double 
int 
int 
int 
int 
int 
int 

struct  polyhed 


cost; 
estlmated_cost; 
total_cost; 
ray_dlrection ; 
type_tangent; 
plane_number  ; 
when_path_f ound ; 
ableSexpand; 
forward; 
polygon4vert; 
*polygon4 vert_PTR  ; 


paths ; 


EXTERN  path_and_node  list  *head_path_node_list  INIT(NULL); 

EXTERN  paths  *start_path  INIT(NULL); 

EXTERN  paths  *shortest_path  INIT(NULL); 

typedef  struct  posslble_paths  { 
point  pt;  ~ 

struct  po8sible_paths  *next_node; 
struct  pos8ible_paths  *next_after_node; 
int  polygonnumber; 

]  a__path,  goals; 

typedef  struct  vertical ( 

double  distance; 

struct  pos8ible_paths  *next_node; 

J  vertical_path; 

EXTERN  goals  *goal_path  INIT(NULL); 

typedef  struct  polyhed t 
double  X, 

y; 

int  typetangent; 

) polyhed4 ; 

typedef  struct  boundryllrolts  ( 


double 

xl; 

double 

x2; 

double 

yi; 

double 

y2; 

double 

zl; 

double 

z2; 

) 

boundr ies 

EXTERN  boundrles 

boundry ; 
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EXTERN  char  file[ll]; 

EXTERN  char  ‘filename  INIT(file); 

EXTERN  Int  paths_found  INIT(O); 

EXTERN  int  plane_number  INIT(O); 

EXTERN  int  prlnt2file  INIT(O); 

EXTERN  int  horizontal  INIT(O); 

EXTERN  int  plot2screen  INIT(O); 

EXTERN  int  on_same_polygon  INIT(O); 
EXTERN  int  number_polygons ; 

EXTERN  int  number_path8_found  INIT(O); 
EXTERN  double  goal_dlrection  INIT(O); 
EXTERN  EILE  *fpt2  INIT(NULL); 

/*  function  declarations  */ 


extern  active_node8  *in8ert_nodes_into_active_node_list( ) ; 


extern  a_path  *free_tangent_list( ) ; 
extern  a_path  *make_tangent_list( ) ; 
extern  a_path  *tangent(); 
extern  a_path  ‘visibility () ; 

extern  boundries  assign_boundry_points ( ) ; 


extern  double 
extern  double 
extern  double 
extern  double 


function_one( )  ; 
functlon_two( ) ; 
norm( ) ; 
order ( ) ; 


extern  int 
extern  int 
extern  int 
extern  int 
extern  int 
extern  int 
extern  int 
extern  int 
extern  int 
extern  int 
extern  int 
extern  int 
extern  int 


boundry_check  ( ) ; 
check_active_node_list( ) ; 
check_for_node_on_polygon ( ) ; 
cros8_polygon ( ) ; 
cross_polygon_node_on ( ) ; 
count_polygon_nodes ( ) ; 
decisiont  > ; 

intersect_polyhedron ( ) ; 
lines_intersect( ) ; 
point_on_line( ) ; 

vertical_node_direction_heuristic { ) ; 
direction_heur istic_from_start( ) ; 
direction_heur istic ( ) ; 


extern  equation  cros8_product( ) ; 
extern  equation  f ind_plane_equation{ ) ; 


extern  double  calculate_cost( ) ; 
extern  double  distance(); 


char 


‘malloc( ) ; 


extern  obstacle_li8t  ‘create_obstacle_list( ) ; 
extern  obstacle_plane  ‘create_list( ) ; 


extern  paths 
extern  paths 
extern  paths 
extern  paths 
extern  paths 
extern  paths 
extern  paths 
extern  paths 
extern  paths 
extern  paths 
extern  paths 
extern  paths 
extern  paths 


‘add_8tart_point_to_path( ) ; 
‘create_path_n_nodes ( ) ; 
‘connect_links_of_path_on_polygon{ ) ; 
‘connect_shortest_paths{  ); 
‘contlnue_vertical_path( ); 
‘delete_before_f Inal ( ) ; 
‘dupllcate_entrles( ) ; 

‘dupl icate_paths ( ) ; 
‘expand_paths_wlth_values { ) ; 
‘extend_path( ) ; 

‘f lnd_tangents_from_inner_node( ) ; 
‘goal_reached_update( ) ; 
‘insert_pt_into_list( ) ; 
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extern  paths 
extern  paths 
extern  paths 
extern  paths 
extern  paths 
extern  paths 
extern  paths 
extern  paths 
extern  paths 
extern  paths 
extern  paths 
extern  paths 
extern  paths 
extern  paths 


•  insert_verticle_list(  ); 

*lnsert_vertical_list_into_shortest_path( ) ; 
*inark_old_path_wlth_node_unexpandable( )  ; 
*niark_shortest_path_last_node( )  ; 
*up_and_over ( ) ; 

*up_and_over_f romgoal ( ) ; 

*up_around_up( ) ; 

*recalculate_vertlcal_path( ) ; 
*reniove_8hortest_path( ) ; 

*reinove_paths  (  ) ; 

*reversed_list( ) ; 

•8tart_flnding_paths( ) ; 
*update_current_path( ) ; 

•update ( ) ; 


extern  point 
extern  point 
extern  point 
extern  point 
extern  point 
extern  point 
extern  point 
extern  point 
extern  point 
extern  point 


a88ign_polnt_values ( ) ; 
deterinlne_end_polnt( ) ; 
flnd_8horte8t_path( ) ; 
lncrea8e_goal_x( ) ; 
lnput_point( ) ; 
inter8ectlon_polnt ( ) ; 
plU8_tangent ( ) ; 
iBinu8_tangent  ( ) ; 

f lnd_point_on_obstacle_plane_face( ) ; 
8wap( ); 


extern  polygon_llst  *add_on_new_plane_intersection( )  ; 

extern  polygon_llst  *build_polygon( ) ; 

extern  polygon_list  *build_two_d_poiygon_list( ) ; 

extern  polygon_list  •connect_llnks ( ) ; 

extern  polygon_llst  •frep_polygon_ilst( ); 

extern  polygon_li8t  *iiiake_list( ) ; 

extern  polygon_list  *remove_dupiicate_nodes( ) ; 

extern  polygon_li8t  *reinove_la8t_polygon{ ) ; 

extern  polygon_li8t  *remove_planes ( ) ; 

extern  polyhedron_face  ‘f ind_polyhedron_face( ) ; 

extern  polyhedron_obstacle  *create_face( ) ; 
extern  polyhedron_ob8tacle_plane  *create_plane( ) ; 

extern  polyhed4  *lnput_data4polyhed4vert( ) ; 


extern  void 
extern  void 
extern  void 
extern  void 
extern  void 
extern  void 
extern  void 
extern  void 
extern  void 
extern  void 
extern  void 
extern  void 
extern  void 
extern  void 
extern  void 


read_cominent( )  ; 
f ind_all_paths ( ) ; 
f  ree_meinory  (  ) ; 
input_start_goal ( ) ; 
inside_boundries ( ) ; 
load_goal_path( ) ; 

vertlcal_n_perpendlcular_plane( ) ; 
prlnt_intersection( ) ; 
prlnt_result8{ ) ; 
print_2d_path( ) ; 
pr lnt_each_paths ( ) ; 
print_path( ) ; 
print_path_8hortest( ) ; 
print_polyhedron{ ) ; 
print_path_to_f iie( ) ; 


extern  vertical_path 
extern  vertlcal_path 
extern  ver tical_path 
extern  vertlcal_path 


*f ind_vertical_nodes ( ) ; 

•new_vertlcal_new_llst( ); 
*trim_not_needed_nodes ( ) ; 
*vertical_node_list_from_pt ( ); 
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PLOT.H 


# include  <graphics . h> 

((include  <dos .  h> 

#deflne  DOS 
#deflne  ESC 
«define  SCALEDX 
((define  SCALEDY 
#define  DOT 
((define  RED 
#define  YELLOW 
#define  REGULAR 

#ifdef  MAIN 

#  define  EXTERN 
((  define  INIT(Value)  =  Value 
((else 

((  define  EXTERN  extern 

#  define  INIT(Value) 

((endif 

EXTERN  int  GraphDriver  INIT<0);/*  The  Graphics  device  driver*/ 

EXTERN  int  GraphMode  INIT(O);/*  The  Graphics  mode  value*/ 

EXTERN  int  MaxX,  MaxY  INIT(O);/*  The  maximum  resolution  of  the  screen  */ 
EXTERN  int  MaxColora  INIT(O);/*  The  maximum  #  of  colors  available*/ 

EXTERN  int  ErrorCode  INIT(O);/*  Reports  any  graphics  errors*/ 

EXTERN  double  AspectRatio  INIT(O.O);/*  Aspect  ratio  of  a  pixel  on  the  screen*/ 
EXTERN  struct  palettetype  palette;/*  Used  to  read  palette  info*/ 

EXTERN  int  txpansion_lable_x  INIT(O);/*  Used  to  label  the  expansion  point*/ 
EXTERN  int  Expans ion_lable_y  INIT(O); 

extern  void  data_plot(); 
extern  void  Initialize^); 
extern  void  Pause(); 
extern  void  MainWindow( ) ; 
extern  void  DrawBorder ( ) ; 
extern  void  StatusLineO; 
extern  void  changetextstyle( ) ; 
extern  void  draw_boundry ( ) ; 
extern  void  drawpolygons ( ) ; 
extern  void  draw_shortest_path( ) ; 

extern  void  draw_line_segment( ) ; 

extern  void  id_shorte8t_path_for_expansion( ) ; 


1 

Oxlb/*  Define  the  escape  key*/ 

X  /  48 
y  /  34 

1  /*used  to  make  a  dot  with  radius  1*/ 

12 
14 
0 
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APPENDIX  M 


This  first  example  is  for  1  polyhedron  and  corresponds  to  the  first  example  in 
Chapter  IV. 

INITIAL  PARTIAL  PATHS 

PATH  1 

PATH  NUMBER  0  Path  able  to  extend 

cost  -  11.589,  est.cost  -  31.192,  total  =  42.781 

Plus  Tangent,  direction  =  0.661 
(10.00,  20.00,  7.08)  to 
(1.00,  13.00,  5.00) 


PATH  2 

PATH  NUMBER  1  Path  able  to  extend 

cost  =  11.014,  est.cost  =  31.811,  total  =  42.825 

Minus  Tangent,  direction  =  -0.588 
(10.00,  7.00,  7.08)  to 
(1.00,  13.00,  5.00) 


PATH  3 

PATH  NUMBER  2  Path  able  to  extend 

cost  =  13.236,  est.cost  =  28.263,  total  =  41.499 

Minus  Tangent,  direction  =■  0.051 
(14.00,  13.67,  3.00)  to 
(10.00,  13.46,  3.00)  to 
(1.00,  13.00,  5.00) 


THE  GOAL  HAS  BEEN  REACHED 
PATH  FINDING  ITERATION  COUNT  =  2 
PATH  1 

PATH  NUMBER  0  Path  able  to  extend 

cost  -  11.589,  est.cost  =  31.192,  total  =  42.781 

Plus  Tangent,  direction  =  0.661 
(10.00,  20.00,  7.08)  to 
(1.00,  13.00,  5.00) 


PATH  2 

PATH  NUMBER  1  Path  able  to  extend 

cost  •=  11.014,  est.cost  =  31.811,  total  =  42.82.S 

Minus  Tangent,  direction  -  -0.588 
(10.00,  7.00,  7.08)  to 
(1.00,  13.00,  5.00) 


PATH  3 

PATH  NUMBER  2  Path  able  to  extend 

cost  -  13.236,  est.cost  =  28.263,  total  =  41.499 


Minus  Tangent,  direction  =  0.051 
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(14.00,  13.67,  3.00)  to 
(10.00,  13.46,  3.00)  to 
(1.00,  13.00,  5.00) 

SHORTEST  PATH 

(14.00,  13.67,  3.00)  to 
(10.00,  13.46,  3.00)  to 
(1.00,  13.00,  5.00) 

The  total  length  of  the  shortest  path  is  41.499 


PATH  FINDING  ITERATION  COUNT  -  3 
PATH  1 

PATH  NUMBER  0  Path  able  to  extend 

cost  •  11.589,  est.cost  =  31.192,  total  =  42.781 

Plus  Tangent,  direction  =  0.661 
(10.00,  20.00,  7.08)  to 
(1.00,  13.00,  5.00) 


PATH  2 

PATH  NUMBER  1  Path  able  to  extend 

cost  =  11.014,  est.cost  =  31.811,  total  =  42.825 

Minus  Tangent,  direction  =  -0.588 
(10.00,  7.00,  7.08)  to 
(1.00,  13.00,  5.00) 


PATH  3 

PATH  NUMBER  2  Path  able  to  extend 

cost  -  41.499,  est.cost  =  0.000,  total  ■=  41.499 


Minus  Tangent, 
(40.00,  15.00, 
(40.00,  15.00, 
(14.00,  13.67, 
(10.00,  13.46, 
(1.00,  13.00, 
SHORTEST  PATH 


direction 
14.00)  to 
14.00)  to 
3.00)  to 
3.00)  to 
5.00) 


0.051 


(40.00,  15.00,  14.00)  to 
(40.00,  15.00,  14.00)  to 
(14.00,  13.67,  3.00)  to 
(10.00,  13.46,  3.00)  to 
(1.00,  13.00,  5.00) 

The  total  length  of  the  shortest  path  is  41.499 


This  example  is  the  listing  generated  by  the  algorithm  for  the  second  example  for 
a  one  polyhedral  world  in  Chapter  IV. 

INITIAL  PARTIAL  PATHS 

PATH  1 

PATH  NUMBER  0  Path  able  to  extend 

cost  -  11.589,  est.cost  =  31.192,  total  =  42.781 

Plus  Tangent,  direction  -=  0.661 
(10.00,  20.00,  7.08)  to 
(1.00,  13.00,  5.00) 


PATH  2 

PATH  NUMBER  1  Path  able  to  extend 

cost  -  11.014,  est.cost  =  31.811,  total  =  42.825 
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Minus  Tangent,  direction  =  -0.588 
(10.00,  7.00,  7.08)  to 
(1.00,  13.00,  5.00) 


PATH  3 

PATH  NUMBER  2  Path  able  to  extend 

cost  “  13.236,  est.cost  =  28.263,  total  =  41.499 

Minus  Tangent,  direction  =  0.051 
(14.00,  13.67,  3.00)  to 
(10.00,  13.46,  3.00)  to 
(1.00,  13.00,  5.00) 


THE  GOAL  HAS  BEEN  REACHED 
PATH  FINDING  ITERATION  COUNT  =  2 
PATH  1 

PATH  NUMBER  0  Path  able  to  extend 

cost  -  11.589,  est.cost  =  31.192,  total  =  42.781 

Plus  Tangent,  direction  =  0.661 
(10.00,  20.00,  7.08)  to 
(1.00,  13.00,  5.00) 


PATH  2 

PATH  NUMBER  1  Path  able  to  extend 

cost  -  11.014,  est.cost  =  31.811,  total  =  42.825 

Minus  Tangent,  direction  *=  -0.588 
(10.00,  7. CO,  7.08)  to 
(1.00,  13.00,  5.00) 


PATH  3 

PATH  NUMBER  2  Path  able  to  extend 

cost  -  13.236,  est.cost  =  28.263,  total  *  41.499 

Minus  Tangent,  direction  =  0.051 
(14.00,  13.67,  3.00)  to 
(10.00,  13.46,  3.00)  to 
(1.00,  13.00,  5.00) 

SHORTEST  PATH 

(14.00,  13.67,  3.00)  to 
(10.00,  13.46,  3.00)  to 
(1.00,  13.00,  5.00) 

The  total  length  of  the  shortest  path  is  41.499 


PATH  FINDING  ITERATION  COUNT  =  3 
PATH  1 

PATH  NUMBER  0  Path  able  to  extend 

cost  -  11.589,  est.cost  =  31.192,  total  =  42.781 

Plus  Tangent,  direction  -  0.661 
(10.00,  20.00,  7.08)  to 
(1.00,  13.00,  5.00) 


PATH  2 

PATH  NUMBER  1  Path  able  to  extend 

cost  *  11.014,  est.cost  =  31.811,  total  =  42.825 
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Minus  Tangent,  direction  =  -0.588 
(10.00,  7.00,  7.08)  to 
(1.00,  13.00,  5.00) 


PATH  3 

PATH  NUMBER  2  Path  able  to  extend 

cost  “  41.499,  est.cost  -  0.000,  total  =  41.499 


Minus  Tangent, 
(40.00,  15.00, 
(40.00,  15.00, 
(14.00,  13.67, 
(10.00,  13.46, 
(1.00,  13.00, 
SHORTEST  PATH 


direction 
14.00)  to 
14.00)  to 
3.00)  to 
3.00)  to 
5.00) 


0.051 


(40.00,  15.00,  14.00)  to 
(40.00,  15.00,  14.00)  to 
(14.00,  13.67,  3.00)  to 
(10.00,  13.46,  3.00)  to 
(1.00,  13.00,  5.00) 

The  total  length  of  the  -shortest  path  is  41.499 


This  next  example  corresponds  to  the  first  example  used  in  a  two  polyhedral  world 
in  Chapter  IV. 

INITIAL  PARTIAL  PATHS 


PATH  1 

PATH  NUMBER  0  Path  able  to  extend 

cost  -  10.296,  est.cost  =  30.414,  total  =  40.709 

Plus  Tangent,  direction  »  0.507 
(10.00,  20.00,  14.00)  to 
(1.00,  15.00,  14.00) 


PATH  2 

PATH  NUMBER  1  Path  able  to  extend 

cost  -  12.042,  est.cost  =  31.048,  total  =  43.090 

Minus  Tangent,  direction  =  -0.727 
(10.00,  7.00,  14.00)  to 
(1.00,  15.00,  14.00) 


PATH  3 

PATH  NUMBER  2  Path  able  to  extend 

cost  -  18.545,  est.cost  -  28.438,  total  =  46.983 

Minus  Tangent,  direction  =•  0.257 
(14.00,  18.42,  3.00)  to 
(10.00,  17.37,  3.00)  to 
(1.00,  15.00,  14.00) 


PATH  4 

PATH  NUMBER  3  Path  able  to  extend 

cost  “  19.049,  est.cost  =  28.757,  total  >=  47.806 

Minus  Tangent,  direction  =  -0.399 
(14.00,  9.53,  3.00)  to 
(10.00,  11.21,  3.00)  to 
(1.00,  15.00,  14.00) 
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PATH  5 

PATH  NUMBER  4  Path  able  to  extend 

cost  »  28.213/  est.cost  =  19.416,  total  =  47.629 

Minus  Tangent,  direction  =  0.000 
(24.00,  15.00,  3.00)  to 
(20.00,  15.00,  3.00)  to 
(14.00,  15.00,  3.00)  to 
(10.00,  15.00,  3.00)  to 
(1.00,  15.00,  14.00) 


PATH  6 

PATH  NUMBER  5  Path  able  to  extend 

cost  -  24.216,  est.cost  =  23.640,  total  =  47.856 

Minus  Tangent,  direction  =  -2.843 
(20.00,  8.85,  3.00)  to 
(24.00,  10.08,  3.00)  to 
(40.00,  15.00,  14.00) 


PATH  7 

PATH  NUMBER  6  Path  able  to  extend 

cost  -  23.732,  est.cost  =  23.147,  total  =  46.879 

Minus  Tangent,  direction  -=  2.952 
(20.00,  18.85,  3.00)  to 
(24.00,  18.08,  3.00)  to 
(40.00,  15.00,  14.00) 

SHORTEST  PATH 

(10.00,  20.00,  14.00)  to 
(1.00,  15.00,  14.00) 

The  total  length  of  the  shortest  path  is  40.709 


THE  GOAL  HAS  BEEN  REACHED 
PATH  FINDING  ITERATION  COUNT  =  2 
PATH  1 

PATH  NUMBER  0  Path  cannot  be  extended 

cost  -  10.296,  est.cost  -  30.414,  total  =  40.709 

Plus  Tangent,  direction  -  0.507 
(10.00,  20.00,  14.00)  to 
(1.00,  15.00,  14.00) 


PATH  2 

PATH  NUMBER  10  Path  able  to  extend 

cost  =  28.613,  est.cost  =  21.541,  total  =  50.154 

Minus  Tangent,  direction  =  -1.138 
(20.00,  7.00,  14.00)  to 
(14.00,  20.00,  14.00)  to 
(10.00,  20.00,  14.00)  to 
(1.00,  15.00,  14.00) 


PATH  3 

PATH  NUMBER  8  Path  able  to  extend 

cost  -  20.296,  est.cost  -  20.616,  total  »  40.911 

Plus  Tangent,  direction  =  0.000 
(20.00,  20.00,  14.00)  to 
(14.00,  20.00,  14.00)  to 
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(10.00,  20.00 

(1.00,  15.00, 


PATH  4 

PATH  NUMBER  1 
cost  -  12.042 

Minus  Tangent 
(10.00,  7.00, 
(1.00,  15.00, 


PATH  5 

PATH  NUMBER  2 
cost  -  18.545 

Minus  Tangent 
(14.00,  18.42 
(10.00,  17.37 
(1.00,  15.00, 


PATH  6 

PATH  NUMBER  3 
cost  -  19.049 

Minus  Tangent 
(14.00,  9.53, 
(10.00,  11.21 
(1.00,  15.00, 


PATH  7 

PATH  NUMBER  4 
cost  -  28.213 

Minus  Tangent 
(24.00,  15.00 
(20.00,  15.00 
(14.00,  15.00 
(10.00,  15.00 
(1.00,  15.00, 


PATH  8 

PATH  NUMBER  5 
cost  -  24.216 

Minus  Tangent 
(20.00,  8.85, 
(24.00,  10.08 
(40.00,  15.00 


PATH  9 

PATH  NUMBER  6 
cost  -  23.732 

Minus  Tangent 
(20.00,  18.85 
(24.00,  18.08 
(40.00,  15.00 
SHORTEST  PATH 

(20.00,  20.00 
(14.00,  20.00 
(10.00,  20.00 


14.00)  to 
14.00) 


Path  able  to  extend 

est.cost  =  31.048,  total  =  43.090 

direction  =  -0.727 
14.00)  to 
14.00) 


Path  able  to  extend 

est.cost  =  28.438,  total  =  46.983 

direction  =  0.257 
3.00)  to 
3.00)  to 
14.00) 


Path  able  to  extend 

est.cost  =  28.757,  total  =  47.806 

direction  =  -0.399 
3.00)  to 
3.00)  to 
14.00) 


Path  able  to  extend 

esc. cost  -  19.416,  total  ■=  47.629 

direction  *  0.000 
3.00)  to 
3.00)  to 
3.00)  to 
3.00)  to 
14.00) 


Path  able  to  extend 

est.cost  =  23.640,  total  =  47.856 

direction  ‘  -2.843 
3.00)  to 
3.00)  to 
14.00) 


Path  able  to  extend 

est.cost  =  23.147,  total  =  46.879 

direction  =  2.952 
3.00)  to 
3.00)  to 
14.00) 


14.00)  to 
14.00)  to 
14.00)  to 
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(1.00,  15.00,  14.00) 

The  total  length  of  the  shortest  path  is  40.911 


PATH  FINDING  ITERATION  COUNT  =  3 
PATH  1 

PATH  NUMBER  0  Path  cannot  be  extended 

cost  -  10.296,  est.cost  =  30.414,  total  =  40.709 

Plus  Tangent,  direction  =  0.507 
(10.00,  20.00,  14.00)  to 
(1.00,  15.00,  14.00) 


PATH  2 

PATH  NUMBER  10  Path  able  to  extend 

cost  -  28.613,  est.cost  =  21.541,  total  =  50.154 

Minus  Tangent,  direction  -  -1.138 
(20.00,  7.00,  14.00)  to 
(14.00,  20.00,  14.00)  to 
(10.00,  20.00,  14.00)  to 
(1.00,  15.00,  14.00) 


PATH  3 

PATH  NUMBER  8  Path  able  to  extend 

cost  =  41.059,  est.cost  =  0.000,  total  =  41.059 

Minus  Tangent,  direction  =  0.000 


(40 

.00, 

15. 

.00, 

14 

.00) 

to 

(24 

.00, 

20. 

.00, 

14 

.00) 

to 

(20 

.00, 

20. 

.00, 

14 

.00) 

to 

(14 

.00, 

20. 

.00, 

14 

.00) 

to 

(10. 

.00, 

20. 

00, 

14 

.00) 

to 

(1.00,  15.00,  14.00) 


PATH  4 

PATH  NUMBER  1  Path  able  to  extend 

cost  -  12.042,  est.cost  =  31.048,  total  =  43.090 

Minus  Tangent,  direction  =  -0.727 
(10.00,  7.00,  14.00)  to 
(1.00,  15.00,  14.00) 


PATH  5 

PATH  NUMBER  2  Path  able  to  extend 

cost  »  18.545,  est.cost  =  28.438,  total  =  46.983 

Minus  Tangent,  direction  =  0.257 
(14.00,  18.42,  3.00)  to 
(10.00,  17.37,  3.00)  to 
(1.00,  15.00,  14.00) 


PATH  6 

PATH  NUMBER  3  Path  able  to  extend 

cost  -  19.049,  est.cost  •=  28.757,  total  -  47.806 

Minus  Tangent,  direction  «  -0.399 
(14.00,  9.53,  3.00)  to 
(10.00,  11.21,  3.00)  to 
(1.00,  15.00,  14.00) 
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PATH  7 

PATH  NUMBER  4  Path  able  to  extend 

cost  “  28.213,  est.cost  =  19.416,  total  =  47.629 

Minus  Tangent,  direction  =  0.000 
(24.00,  15.00,  3.00)  to 
(20.00,  15.00,  3.00)  to 
(14.00,  15.00,  3.00)  to 
(10.00,  15.00,  3.00)  to 
(1.00,  15.00,  14.00) 


PATH  8 

PATH  NUMBER  5  Path  able  to  extend 

cost  -  24.216,  est.cost  •=  23.640,  total  =  47.856 

Minus  Tangent,  direction  =  -2.843 
(20.00,  8.85,  3.00)  to 
(24.00,  10.08,  3.00)  to 
(40.00,  15.00,  14.00) 


PATH  9 

PATH  NUMBER  6  Path  able  to  extend 

cost  -  23.732,  est.cost  =  23.147,  total  =  46.879 

Minus  Tangent,  direction  =  2.952 
(20.00,  18.85,  3.00)  to 
(24.00,  18.08,  3.00)  to 
(40.00,  15.00,  14.00) 

SHORTEST  PATH 

(40.00,  15.00,  14.00)  to 
(24.00,  20.00,  14.00)  to 
(20.00,  20.00,  14.00)  to 
(14.00,  20.00,  14.00)  to 
(10.00,  20.00,  14.00)  to 
(1.00,  15.00,  14.00) 

The  total  length  of  the  shortest  path  is  41.059 

This  final  example  is  the  listing  generated  by  the  algorithm  for  the  second  example 
for  a  two  polyhedral  world  in  Chapter  IV. 

INITIAL  PARTIAL  PATHS 

PATH  1 

PATH  NUMBER  0  Path  able  to  extend 

cost  -  7.642,  est.cost  =  31.072,  total  *  38.715 

Plus  Tangent,  direction  =  1.166 
(10.00,  20.00,  7.64)  to 
(7.00,  13.00,  7.00) 


PATH  2 

PATH  NUMBER  1  Path  able  to  extend 

cost  “  6.738,  est.cost  -  31.694,  total  •=  38.432 

Minus  Tangent,  direction  =  -1.107 
(10.00,  7.00,  7.64)  to 
(7.00,  13.00,  7.00) 


PATH  3 

PATH  NUMBER  2  Path  able  to  extend 

cost  “  9.797,  est.cost  -  28.287,  total  »  38.084 

Minus  Tangent,  direction  =  0.494 
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(14.00,  16.77,  3.00)  to 
(10.00,  14.62,  3.00)  to 
(7.00,  13.00,  7.00) 


PATH  4 

PATH  NUMBER  3  Path  able  to  extend 

cost  -  9.594,  est.cost  -  28.712,  total  -  38.305 

Minus  Tangent,  direction  •  -0.432 
(14.00,  9.77,  3.00)  to 
(10.00,  11.62,  3.00)  to 
(7.00,  13.00,  7.00) 


PATH  5 

PATH  NUMBER  4  Path  able  to  extend 

cost  -  19.029,  est.cost  “  19.441,  total  =  38.470 

Minus  Tangent,  direction  -  0.061 
(24.00,  14.03,  3.00)  to 
(20.00,  13.79,  3.00)  to 
(14.00,  13.42,  3.00)  to 
(10.00,  13.18,  3.00)  to 
(7.00,  13.00,  7.00) 


PATH  6 

PATH  NUMBER  5  Path  able  to  extend 

cost  -  24.216,  est.cost  =  23.640,  total  =  47.856 

Minus  Tangent,  direction  -  -2.843 
(20.00,  8.85,  3.00)  to 
(24.00,  10.08,  3.00)  to 
(40.00,  15.00,  14.00) 


PATH  7 

PATH  NUMBER  6  Path  able  to  extend 

cost  “  23.732,  est.cost  =  23.147,  total  =  46.879 


Minus  Tangent, 
(20.00,  18.85, 
(24.00,  18.08, 
(40.00,  15.00, 
SHORTEST  PATH 


direction 
3.00)  to 
3.00)  to 
14.00) 


2.952 


(14.00,  16.77,  3.00)  to 
(10.00,  14.62,  3.00)  to 
(7.00,  13.00,  7.00) 

The  total  length  of  the  shortest  path  is  38.084 


PATH  FINDING  ITERATION  COUNT  =  2 
PATH  1 

PATH  NUMBER  0  Path  able  to  extend 

cost  “  7.642,  est.cost  -  31.072,  total  =  38.715 

Plus  Tangent,  direction  -  1.166 
(10.00,  20.00,  7.64)  to 
(7.00,  13.00,  7.00) 


PATH  2 

PATH  NUMBER  1  Path  able  to  extend 

cost  -  6.738,  est.cost  «  31.694,  total  «  38.432 
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Minus  Tangent,  direction  =  -1.107 
(10.00,  7.00,  7.64)  to 
(7.00,  13.00,  7.00) 


PATH  3 

PATH  NUMBER  7  Path  able  to  extend 

cost  -  17.069,  est.cost  •=  22.284,  total  =  39.354 

Plus  Tangent,  direction  »  0.494 
(20.00,  20.00,  5.54)  to 
(14.00,  16.77,  3.00)  to 
(10.00,  14.62,  3.00)  to 
(7.00,  13.00,  7.00) 


PATH  4 

PATH  NUMBER  3  Path  able  to  extend 

cost  -  9.594,  est.cost  =  28.712,  total  =  38.305 

Minus  Tangent,  direction  =  -0.432 
(14.00,  9.77,  3.00)  to 
(10.00,  11.62,  3.00)  to 
(7.00,  13.00,  7.00) 


PATH  5 

PATH  NUMBER  4  Path  able  to  extend 

cost  -  19.029,  est.cost  “  19.441,  total  •=  38.470 

Minus  Tangent,  direction  =  0.061 
(24.00,  14.03,  3.00)  to 
(20.00,  13.79,  3.00)  to 
(14.00,  13.42,  3.00)  to 
(10.00,  13.18,  3.00)  to 
(7.00,  13.00,  7.00) 


PATH  6 

PATH  NUMBER  5  Path  able  to  extend 

cost  -  24.216,  est.cost  =  23.640,  total  =  47.856 

Minus  Tangent,  direction  =  -2.843 
(20.00,  8.85,  3.00)  to 
(24.00,  10.08,  3.00)  to 
(40.00,  15.00,  14.00) 


PATH  7 

PATH  NUMBER  6  Path  able  to  extend 

cost  -  23.732,  est.cost  =  23.147,  total  =  46.879 

Minus  Tangent,  direction  =  2.952 
(20.00,  18.85,  3.00)  to 
(24.00,  18.08,  3.00)  to 
(40.00,  15.00,  14.00) 

SHORTEST  PATH  % 

(14.00,  9.77,  3.00)  to 
(1C. 00,  11.62,  3.00)  to 
(7.00,  13.00,  7.00) 

The  total  length  of  the  shortest  path  is  38.305 


PATH  FINDING  ITERATION  COUNT  =  3 
PATH  1 

PATH  NUMBER  0  Path  able  to  extend 
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cost  -  7.642,  est.cost  “  31.072,  total  =  38.715 


Plus  Tangent,  direction  =  1.166 
(10.00,  20.00,  7.64)  to 
(7.00,  13.00,  7.00) 


PATH  2 

PATH  NUMBER  1  Path  able  to  extend 

cost  -  6.738,  est.cost  =  31.694,  total  =  38.432 

Minus  Tangent,  direction  =  -1.107 
(10.00,  7.00,  7.64)  to 
(7.00,  13.00,  7.00) 


PATH  3 

PATH  NUMBER  7  Path  able  to  extend 

cost  “  17.069,  est.cost  =  22.284,  total  =  39.354 

Plus  Tangent,  direction  »  0.494 
(20.00,  20.00,  5.54)  to 
(14.00,  16.77,  3.00)  to 
(10.00,  14.62,  3.00)  to 
(7.00,  13.00,  7.00) 


PATH  4 

PATH  NUMBER  8  Path  able  to  extend 

cost  ”  16.673,  est.cost  *  23.143,  total  =  39.816 

Minus  Tangent,  direction  =  -0.432 
(20.00,  7.00,  5.54)  to 
(14.00,  9.77,  3.00)  to 
(10.00,  11.62,  3.00)  to 
(7.00,  13.00,  7.00) 


PATH  5 

PATH  NUMBER  4  Path  able  to  extend 

cost  -  19.029,  est.cost  •  19.441,  total  =  38.470 

Minus  Tangent,  direction  =  0.061 
(24.00,  14.03,  3.00)  to 
(20.00,  13.79,  3.00)  to 
(14.00,  13.42,  3.00)  to 
(10.00,  13.18,  3.00)  to 
(7.00,  13.00,  7.00) 


PATH  6 

PATH  NUMBER  5  Path  able  to  extend 

cost  -  24.216,  est.cost  =  23.640,  total  -  47.856 

Minus  Tangent,  direction  -  -2.843 
(20.00,  8.85,  3.00)  to 
(24.00,  10.08,  3.00)  to 
(40.00,  15.00,  14.00) 


PATH  7 

PATH  NUMBER  6  Path  able  to  extend 

cost  -  23.732,  est.cost  -  23.147,  total  -=  46.879 


Minus  Tangent,  direction 
(20.00,  18.85,  3.00)  to 
(24.00,  18.08,  3.00)  to 
(40.00,  15.00,  14.00) 


2.952 
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SHORTEST  PATH 


(10.00,  7.00,  7.64)  to 
(7.00,  13.00,  7.00) 

The  total  length  of  the  shortest  path  is  38.432 


THE  GOAL  HAS  BEEN  REACHED 
PATH  FINDING  ITERATION  COUNT  =  5 
PATH  1 

PATH  NUMBER  0  Path  able  to  extend 

cost  -  7.642,  est.cost  =  31.072,  total  =  38.715 

Plus  Tangent,  direction  =  1.166 
(10.00,  20.00,  7.64)  to 
(7.00,  13.00,  7.00) 


PATH  2 

PATH  NUMBER  1  Path  cannot  be  extended 

cost  -  6.738,  est.cost  =  31.694,  total  =  38.432 

Minus  Tangent,  direction  =  -1.107 
(10.00,  7.00,  7.64)  to 
(7.00,  13.00,  7.00) 


PATH  3 

PATH  NUMBER  12  Path  able  to  extend 

cost  »  16.961,  est.cost  =  21.954,  total  =  38.915 

Minus  Tangent,  direction  =  0.000 


(20.00,  7.00,  9.76)  to 
(14.00,  7.00,  8.48)  to 
(10.00,  7.00,  7.64)  to 
(7.00,  13.00,  7.00) 


PATH  4 

PATH  NUMBER  10  Path  able 
cost  -  25.202,  est.cost  = 

Plus  Tangent,  direction  = 
(20.00,  20.00,  9.76)  to 
(14.00,  7.00,  8.48)  to 
(10.00,  7.00,  7.64)  to 
(7.00,  13.00,  7.00) 


to  extend 

21.048,  total  =  46.249 
1.138 


PATH  5 

PATH  NUMBER  7  Path  able  to  extend 

cost  “  17.069,  est.cost  -  22.284,  total  =  39.354 

Plus  Tangent,  direction  -  0.494 
(20.00,  20.00,  5.54)  to 
(14.00,  16.77,  3.00)  to 
(10.00,  14.62,  3.00)  to 
(7.00,  13.00,  7.00) 


PATH  6 

PATH  NUMBER  8  Path  able  to  extend 

cost  -  16.673,  est.cost  -  23.143,  total  -  39.816 

Minus  Tangent,  direction  =  -0.432 
(20.00,  7.00,  5.54)  to 
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(14.00,  9.77,  3.00)  to 
(10.00,  11.62,  3.00)  to 
(7.00,  13.00,  7.00) 


PATH  7 

PATH  NUMBER  4  Path  able  to  extend 

cost  -  19.029,  est.cost  =  19.441,  total  =  38.470 


Minus  Tangent, 
(24.00,  14.03, 
(20.00,  13.79, 
(14.00,  13.42, 
(10.00,  13.18, 
(7.00,  13.00, 


direction 
3.00)  to 
3.00)  to 
3.00)  to 
3.00)  to 
7.00) 


0.061 


< 


PATH  8 

PATH  NUMBER  5  Path  able  to  extend 

cost  “  24.216,  est.cost  =  23.640,  total  =  47.856 

Minus  Tangent,  direction  =  -2.843 
(20.00.  8.85,  3.00)  to 
(24.00,  10.08,  3.00)  to 
(40.00,  15.00,  14.00) 


PATH  9 

PATH  NUMBER  6  Path  able  to  extend 

cost  “  23.732,  est.cost  *  23.147,  total  =  46.879 


Minus  Tangent,  direction 
(20.00,  18.85,  3.00)  to 
(24.00,  18.08,  3.00)  to 
(40.00,  15.00,  14.00) 
SHORTEST  PATH 


2.952 


(24.00,  14.03,  3.00)  to 
(20.00,  13.79,  3.00)  to 
(14.00,  13.42,  3.00)  to 
(10.00,  13.18,  3.00)  to 
(7.00,  13.00,  7.00) 

The  total  length  of  the  shortest  path  is  38.470 


PATH  FINDING  ITERATION  COUNT  =  6 
PATH  1 

PATH  NUMBER  0  Path  able  to  extend 

cost  -  7.642,  est.cost  =  31.072,  total  -  38.715 

Plus  Tangent,  direction  =  1.166 
(10.00,  20.00,  7.64)  to 
(7.00,  13.00,  7.00) 


PATH  2 

PATH  NUMBER  1  Path  cannot  be  extended 

cost  -  6.738,  est.cost  -  31.694,  total  *  38.432 

Minus  Tangent,  direction  -  -1.107 
(10.00,  7.00,  7.64)  to 
(7.00,  13.00,  7.00) 


PATH  3 

PATH  NUMBER  12  Path  able  to  extend 

cost  -  16.961,  est.cost  -  21.954,  total  =  38.915 
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Minus  Tangent,  direction  =  0.000 
(20.00,  7.00,  9.76)  to 
(14.00,  7.00,  8.48)  to 
(10.00,  7.00,  7.64)  to 
(7.00,  13.00,  7.00) 


PATH  4 

PATH  NUMBER  10  Path  able  to  extend 
cost  -  25.202,  est.cost  =  21.048,  total 

Plus  Tangent,  direction  =  1.138 
(20.00,  20.00,  9.76)  to 
(14.00,  7.00,  8.48)  to 
(10.00,  7.00,  7.64)  to 
(7.00,  13.00,  7.00) 


PATH  5 

PATH  NUMBER  7  Path  able  to  extend 
cost  -  17.069,  est.cost  -  22.284,  total 

Plus  Tangent,  direction  =  0.494 
(20.00,  20.00,  5.54)  to 
(14.00,  16.77,  3.00)  to 
(10.00,  14.62,  3.00)  to 
(7.00,  13.00,  7.00) 


PATH  6 

PATH  NUMBER  8  Path  able  to  extend 
cost  -  16.673,  est.cost  =  23.143,  total 

Minus  Tangent,  direction  =  -0.432 
(20.00,  7.00,  5.54)  to 
(14.00,  9.77,  3.00)  to 
(10.00,  11.62,  3.00)  to 
(7.00,  13.00,  7.00) 


PATH  7 

PATH  NUMBER  4  Path  able  to  extend 
cost  “  38.470,  est.cost  =  0.000,  total  = 

Minus  Tangent,  direction  =  0.061 
(40.00,  15.00,  14.00)  to 
(40.00,  15.00,  14.00)  to 
(24.00,  14.03,  3.00)  to 
(20.00,  13.79,  3.00)  to 
(14.00,  13.42,  3.00)  to 
(10.00,  13.18,  3.00)  to 
(7.00,  13.00,  7.00) 


PATH  8 

PATH  NUMBER  5  Path  able  to  extend 
cost  -  24.216,  est.cost  =  23.640,  total 

Minus  Tangent,  direction  =  -2.843 
(20.00,  8.85,  3.00)  to 
(24.00,  10.08,  3.00)  to 
(40.00,  15.00,  14.00) 


PATH  9 

PATH  NUMBER  6  Path  able  to  extend 
cost  -  23.732,  est.cost  =  23.147,  total 


=  46.249 


-  39.354 


=  39.816 


38.470 


=  47.856 


=  46.879 


167 


Minus  Tangent,  direction  =  2.952 
(20.00,  18.85,  3.00)  to 
(24.00,  18.08,  3.00)  to 
(40.00,  15.00,  14.00) 

SHORTEST  PATH 

(40.00,  15.00,  14.00)  to 
(40.00,  15.00,  14.00)  to 
(24.00,  14.03,  3.00)  to 

(20.00,  13.79,  3.00)  to  % 

(14.00,  13.42,  3.00)  to 
(10.00,  13.18,  3.00)  to 
(7.00,  13.00,  7.00) 

The  total  length  of  the  shortest  path  is  38.470 
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